Fsm pipeline task

auv_setup/config/robots/orca.yaml

@@ -141,3 +141,6 @@
         docking_station_offset: -1.0
         num_measurements: 5
         dock_wait_time: 10.0
+      pipeline:
+        pipeline_offset: -1.0
+        num_measurements: 5

mission/FSM/pipeline/CMakeLists.txt

@@ -0,0 +1,63 @@
+cmake_minimum_required(VERSION 3.5)
+project(pipeline)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(yasmin REQUIRED)
+find_package(yasmin_ros REQUIRED)
+find_package(yasmin_viewer REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(spdlog REQUIRED)
+find_package(fmt REQUIRED)
+
+include_directories(include)
+
+set(LIB ${CMAKE_PROJECT_NAME}_lib)
+set(DEPENDENCIES
+  rclcpp
+  yasmin
+  yasmin_ros
+  yasmin_viewer
+  vortex_msgs
+  std_msgs
+  spdlog
+  fmt
+)
+
+
+add_executable(pipeline
+  src/pipeline.cpp
+)
+
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_target_dependencies(pipeline ${DEPENDENCIES})
+target_link_libraries(pipeline fmt::fmt)
+install(TARGETS
+  pipeline
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_export_include_directories(include)
+ament_export_libraries(${LIB})
+
+ament_package()

mission/FSM/pipeline/include/pipeline/pipeline.hpp

@@ -0,0 +1,126 @@
+#ifndef VORTEX_PIPELINE_HPP
+#define VORTEX_PIPELINE_HPP
+
+#include <chrono>
+#include <iostream>
+#include <memory>
+#include <string>
+
+#include <vortex_msgs/action/filtered_pose.hpp>
+#include <vortex_msgs/action/los_guidance.hpp>
+#include <vortex_msgs/action/reference_filter_waypoint.hpp>
+
+#include <std_msgs/msg/string.hpp>
+#include <yasmin/cb_state.hpp>
+#include <yasmin/logs.hpp>
+#include <yasmin/state_machine.hpp>
+#include <yasmin_ros/action_state.hpp>
+#include <yasmin_ros/basic_outcomes.hpp>
+#include <yasmin_ros/ros_logs.hpp>
+#include <yasmin_ros/yasmin_node.hpp>
+#include <yasmin_viewer/yasmin_viewer_pub.hpp>
+
+#include <spdlog/spdlog.h>
+
+namespace pipeline_fsm {
+using PoseStamped = geometry_msgs::msg::PoseStamped;
+using Pose = geometry_msgs::msg::Pose;
+using PointStamped = geometry_msgs::msg::PointStamped;
+
+using FindPipelineAction = vortex_msgs::action::FilteredPose;
+using ApproachPipelineAction = vortex_msgs::action::ReferenceFilterWaypoint;
+using FollowPipelineAction = vortex_msgs::action::ReferenceFilterWaypoint;
+using ConvergeDockingAction = vortex_msgs::action::ReferenceFilterWaypoint;
+using ReturnHomeAction = vortex_msgs::action::ReferenceFilterWaypoint;
+
+}  // namespace pipeline_fsm
+
+class FindPipelineState
+    : public yasmin_ros::ActionState<pipeline_fsm::FindPipelineAction> {
+   public:
+    FindPipelineState(
+        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+
+    pipeline_fsm::FindPipelineAction::Goal create_goal_handler(
+        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+
+    std::string response_handler(
+        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        pipeline_fsm::FindPipelineAction::Result::SharedPtr response);
+
+    void print_feedback(
+        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        std::shared_ptr<const pipeline_fsm::FindPipelineAction::Feedback>
+            feedback);
+};
+
+class ApproachPipelineState
+    : public yasmin_ros::ActionState<pipeline_fsm::ApproachPipelineAction> {
+   public:
+    ApproachPipelineState(
+        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+
+    pipeline_fsm::ApproachPipelineAction::Goal create_goal_handler(
+        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+
+    std::string response_handler(
+        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        pipeline_fsm::ApproachPipelineAction::Result::SharedPtr response);
+
+    void print_feedback(
+        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        std::shared_ptr<const pipeline_fsm::ApproachPipelineAction::Feedback>
+            feedback);
+};
+
+class FollowPipelineState
+    : public yasmin_ros::ActionState<pipeline_fsm::FollowPipelineAction> {
+   public:
+    FollowPipelineState(
+        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+
+    pipeline_fsm::FollowPipelineAction::Goal create_goal_handler(
+        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+
+    std::string response_handler(
+        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        pipeline_fsm::FollowPipelineAction::Result::SharedPtr response);
+
+    void print_feedback(
+        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        std::shared_ptr<const pipeline_fsm::FollowPipelineAction::Feedback>
+            feedback);
+};
+
+class ReturnHomeState
+    : public yasmin_ros::ActionState<pipeline_fsm::ReturnHomeAction> {
+   public:
+    ReturnHomeState(std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+
+    pipeline_fsm::ReturnHomeAction::Goal create_goal_handler(
+        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+
+    std::string response_handler(
+        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        pipeline_fsm::ReturnHomeAction::Result::SharedPtr response);
+
+    void print_feedback(
+        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        std::shared_ptr<const pipeline_fsm::ReturnHomeAction::Feedback>
+            feedback);
+};
+
+std::string AbortState(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+
+std::string ErrorState(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+
+std::shared_ptr<yasmin::StateMachine> create_state_machines();
+
+void add_states(std::shared_ptr<yasmin::StateMachine> sm,
+                std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+
+auto initialize_blackboard();
+
+#endif

mission/FSM/pipeline/launch/YASMIN_viewer.launch.py

@@ -0,0 +1,20 @@
+import launch.actions
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    """Launch file to launch the yasmin viewer."""
+    yasmin_viewer_node = Node(
+        package='yasmin_viewer',
+        executable='yasmin_viewer_node',
+        name='yasmin_viewer',
+        namespace="orca",
+        on_exit=launch.actions.LogInfo(msg="Yasmin_viewer exited"),
+    )
+
+    return LaunchDescription(
+        initial_entities=[
+            yasmin_viewer_node,
+        ],
+    )

mission/FSM/pipeline/launch/pipeline.launch.py

@@ -0,0 +1,36 @@
+import os
+
+import launch.actions
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    """Launch file to launch all nodes necessary for pipeline fsm."""
+    orca_config = os.path.join(
+        get_package_share_directory(package_name='auv_setup'),
+        'config',
+        'robots',
+        'orca.yaml',
+    )
+    pose_config = os.path.join(
+        get_package_share_directory(package_name='pose_action_server'),
+        'config',
+        'pose_action_server_params.yaml',
+    )
+
+    pipeline_launch = Node(
+        package='pipeline',
+        executable='pipeline',
+        namespace="orca",
+        parameters=[orca_config, pose_config],
+        on_exit=launch.actions.LogInfo(msg="Pipeline exited"),
+        output='screen',
+    )
+
+    return LaunchDescription(
+        initial_entities=[
+            pipeline_launch,
+        ],
+    )

mission/FSM/pipeline/package.xml

@@ -0,0 +1,23 @@
+<?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>pipeline</name>
+  <version>0.0.0</version>
+  <description>Finite state machine for pipeline task</description>
+  <maintainer email="[email protected]">hakonts</maintainer>
+  <license>MIT</license>
+
+  <depend>ament_cmake</depend>
+
+  <depend>rclcpp</depend>
+  <depend>yasmin</depend>
+  <depend>yasmin_ros</depend>
+  <depend>yasmin_viewer</depend>
+  <depend>vortex_msgs</depend>
+  <depend>std_msgs</depend>
+
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>