diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml
index 47039a4b..e7c6059c 100644
--- a/auv_setup/config/robots/orca.yaml
+++ b/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
diff --git a/mission/FSM/pipeline/CMakeLists.txt b/mission/FSM/pipeline/CMakeLists.txt
new file mode 100644
index 00000000..0776cee4
--- /dev/null
+++ b/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()
diff --git a/mission/FSM/pipeline/include/pipeline/pipeline.hpp b/mission/FSM/pipeline/include/pipeline/pipeline.hpp
new file mode 100644
index 00000000..7d52d4db
--- /dev/null
+++ b/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
diff --git a/mission/FSM/pipeline/launch/YASMIN_viewer.launch.py b/mission/FSM/pipeline/launch/YASMIN_viewer.launch.py
new file mode 100644
index 00000000..dabeef24
--- /dev/null
+++ b/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,
+        ],
+    )
diff --git a/mission/FSM/pipeline/launch/pipeline.launch.py b/mission/FSM/pipeline/launch/pipeline.launch.py
new file mode 100644
index 00000000..4df9c86d
--- /dev/null
+++ b/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,
+        ],
+    )
diff --git a/mission/FSM/pipeline/package.xml b/mission/FSM/pipeline/package.xml
new file mode 100644
index 00000000..5ad2a59f
--- /dev/null
+++ b/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="hakonts@icloud.com">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>
diff --git a/mission/FSM/pipeline/src/pipeline.cpp b/mission/FSM/pipeline/src/pipeline.cpp
new file mode 100644
index 00000000..67f44f4a
--- /dev/null
+++ b/mission/FSM/pipeline/src/pipeline.cpp
@@ -0,0 +1,411 @@
+#include "pipeline/pipeline.hpp"
+
+using std::placeholders::_1;
+using std::placeholders::_2;
+
+FindPipelineState::FindPipelineState(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard)
+    : yasmin_ros::ActionState<pipeline_fsm::FindPipelineAction>(
+          blackboard->get<std::string>("pose_action"),
+          std::bind(&FindPipelineState::create_goal_handler, this, _1),
+          std::bind(&FindPipelineState::response_handler, this, _1, _2),
+          std::bind(&FindPipelineState::print_feedback, this, _1, _2)) {};
+
+pipeline_fsm::FindPipelineAction::Goal FindPipelineState::create_goal_handler(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) {
+    auto goal = pipeline_fsm::FindPipelineAction::Goal();
+    goal.num_measurements = blackboard->get<bool>("num_measurements");
+
+    spdlog::info("Goal sent to action server:");
+    spdlog::info("  Number of measurements: {}", goal.num_measurements);
+
+    return goal;
+}
+
+std::string FindPipelineState::response_handler(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+    pipeline_fsm::FindPipelineAction::Result::SharedPtr response) {
+    blackboard->set<pipeline_fsm::PoseStamped>("pipeline_start_pose",
+                                               response->filtered_pose);
+
+    spdlog::info("Response received from action server:");
+    spdlog::info("Pipeline start pose: x = {}, y = {}, z = {}",
+                 response->filtered_pose.pose.position.x,
+                 response->filtered_pose.pose.position.y,
+                 response->filtered_pose.pose.position.z);
+
+    pipeline_fsm::PoseStamped pipeline_offset_goal;
+    pipeline_offset_goal = response->filtered_pose;
+    pipeline_offset_goal.pose.position.z +=
+        blackboard->get<double>("pipeline_offset");
+    blackboard->set<pipeline_fsm::PoseStamped>("pipeline_offset_goal",
+                                               pipeline_offset_goal);
+
+    return yasmin_ros::basic_outcomes::SUCCEED;
+}
+
+void FindPipelineState::print_feedback(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+    std::shared_ptr<const pipeline_fsm::FindPipelineAction::Feedback>
+        feedback) {
+    blackboard->set<pipeline_fsm::Pose>("current_pose",
+                                        feedback->current_pose.pose);
+    spdlog::debug("Current position: x = {}, y = {}, z = {}",
+                  feedback->current_pose.pose.position.x,
+                  feedback->current_pose.pose.position.y,
+                  feedback->current_pose.pose.position.z);
+}
+
+ApproachPipelineState::ApproachPipelineState(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard)
+    : yasmin_ros::ActionState<pipeline_fsm::ApproachPipelineAction>(
+          blackboard->get<std::string>("reference_filter_action"),
+          std::bind(&ApproachPipelineState::create_goal_handler, this, _1),
+          std::bind(&ApproachPipelineState::response_handler, this, _1, _2),
+          std::bind(&ApproachPipelineState::print_feedback, this, _1, _2)) {};
+
+pipeline_fsm::ApproachPipelineAction::Goal
+ApproachPipelineState::create_goal_handler(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) {
+    auto goal = pipeline_fsm::ApproachPipelineAction::Goal();
+
+    blackboard->set<bool>("is_home", false);
+
+    pipeline_fsm::PoseStamped pipeline_offset_goal =
+        blackboard->get<pipeline_fsm::PoseStamped>("pipeline_offset_goal");
+
+    goal.goal = pipeline_offset_goal;
+
+    spdlog::info("Goal sent to action server:");
+    spdlog::info("  Position: x = {}, y = {}, z = {}",
+                 pipeline_offset_goal.pose.position.x,
+                 pipeline_offset_goal.pose.position.y,
+                 pipeline_offset_goal.pose.position.z);
+
+    return goal;
+}
+
+std::string ApproachPipelineState::response_handler(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+    pipeline_fsm::ApproachPipelineAction::Result::SharedPtr response) {
+    spdlog::info("Response received from action server:");
+    spdlog::info("  Success: {}", response->success ? "true" : "false");
+
+    blackboard->set<bool>("has_finished_converging", response->success);
+
+    if (blackboard->get<bool>("has_finished_converging")) {
+        return yasmin_ros::basic_outcomes::SUCCEED;
+    } else {
+        return yasmin_ros::basic_outcomes::CANCEL;
+    }
+}
+
+void ApproachPipelineState::print_feedback(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+    std::shared_ptr<const pipeline_fsm::ApproachPipelineAction::Feedback>
+        feedback) {
+    pipeline_fsm::Pose current_pose = pipeline_fsm::Pose();
+    current_pose.position.x = feedback->feedback.x;
+    current_pose.position.y = feedback->feedback.y;
+    current_pose.position.z = feedback->feedback.z;
+
+    blackboard->set<pipeline_fsm::Pose>("current_pose", current_pose);
+
+    spdlog::debug("Current position: x = {}, y = {}, z = {}",
+                  feedback->feedback.x, feedback->feedback.y,
+                  feedback->feedback.z);
+}
+
+FollowPipelineState::FollowPipelineState(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard)
+    : yasmin_ros::ActionState<pipeline_fsm::FollowPipelineAction>(
+          blackboard->get<std::string>("reference_filter_action"),
+          std::bind(&FollowPipelineState::create_goal_handler, this, _1),
+          std::bind(&FollowPipelineState::response_handler, this, _1, _2),
+          std::bind(&FollowPipelineState::print_feedback, this, _1, _2)) {};
+
+pipeline_fsm::FollowPipelineAction::Goal
+FollowPipelineState::create_goal_handler(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) {
+    auto goal = pipeline_fsm::FollowPipelineAction::Goal();
+
+    auto pipeline_offset_goal =
+        blackboard->get<pipeline_fsm::PoseStamped>("pipeline_offset_goal");
+    goal.goal = pipeline_offset_goal;
+
+    spdlog::info("Goal sent to action server:");
+    spdlog::info("  Position: x = {}, y = {}, z = {}",
+                 pipeline_offset_goal.pose.position.x,
+                 pipeline_offset_goal.pose.position.y,
+                 pipeline_offset_goal.pose.position.z);
+    spdlog::info("  Orientation: x = {}, y = {}, z = {}, w = {}",
+                 pipeline_offset_goal.pose.orientation.x,
+                 pipeline_offset_goal.pose.orientation.y,
+                 pipeline_offset_goal.pose.orientation.z,
+                 pipeline_offset_goal.pose.orientation.w);
+    return goal;
+}
+
+std::string FollowPipelineState::response_handler(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+    pipeline_fsm::FollowPipelineAction::Result::SharedPtr response) {
+    spdlog::info("Response received from action server:");
+    spdlog::info("  Success: {}", response->success ? "true" : "false");
+    blackboard->set<bool>("is_finished", response->success);
+
+    if (blackboard->get<bool>("is_finished")) {
+        return yasmin_ros::basic_outcomes::SUCCEED;
+    } else {
+        return yasmin_ros::basic_outcomes::CANCEL;
+    }
+}
+
+void FollowPipelineState::print_feedback(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+    std::shared_ptr<const pipeline_fsm::FollowPipelineAction::Feedback>
+        feedback) {
+    pipeline_fsm::Pose current_pose = pipeline_fsm::Pose();
+    current_pose.position.x = feedback->feedback.x;
+    current_pose.position.y = feedback->feedback.y;
+    current_pose.position.z = feedback->feedback.z;
+
+    blackboard->set<pipeline_fsm::Pose>("current_pose", current_pose);
+
+    spdlog::debug("Current position: x = {}, y = {}, z = {}",
+                  feedback->feedback.x, feedback->feedback.y,
+                  feedback->feedback.z);
+}
+
+ReturnHomeState::ReturnHomeState(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard)
+    : yasmin_ros::ActionState<pipeline_fsm::ReturnHomeAction>(
+          blackboard->get<std::string>("reference_filter_action"),
+          std::bind(&ReturnHomeState::create_goal_handler, this, _1),
+          std::bind(&ReturnHomeState::response_handler, this, _1, _2),
+          std::bind(&ReturnHomeState::print_feedback, this, _1, _2)) {};
+
+pipeline_fsm::ReturnHomeAction::Goal ReturnHomeState::create_goal_handler(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) {
+    auto goal = pipeline_fsm::ReturnHomeAction::Goal();
+
+    blackboard->set<bool>("is_done", false);
+
+    pipeline_fsm::PoseStamped start_pose =
+        blackboard->get<pipeline_fsm::PoseStamped>("start_pose");
+
+    goal.goal = start_pose;
+    spdlog::info("Goal sent to action server:");
+    spdlog::info("  Position: x = {}, y = {}, z = {}",
+                 start_pose.pose.position.x, start_pose.pose.position.y,
+                 start_pose.pose.position.z);
+
+    return goal;
+}
+
+std::string ReturnHomeState::response_handler(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+    pipeline_fsm::ReturnHomeAction::Result::SharedPtr response) {
+    spdlog::info("Response received from action server:");
+    spdlog::info("  Success: {}", response->success ? "true" : "false");
+
+    blackboard->set<bool>("is_home", response->success);
+    if (blackboard->get<bool>("is_home")) {
+        return yasmin_ros::basic_outcomes::SUCCEED;
+    } else {
+        return yasmin_ros::basic_outcomes::CANCEL;
+    }
+}
+
+void ReturnHomeState::print_feedback(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+    std::shared_ptr<const pipeline_fsm::ReturnHomeAction::Feedback> feedback) {
+    pipeline_fsm::Pose current_pose = pipeline_fsm::Pose();
+    current_pose.position.x = feedback->feedback.x;
+    current_pose.position.y = feedback->feedback.y;
+    current_pose.position.z = feedback->feedback.z;
+    current_pose.orientation.x = feedback->feedback.roll;
+    current_pose.orientation.y = feedback->feedback.pitch;
+    current_pose.orientation.z = feedback->feedback.yaw;
+
+    blackboard->set<pipeline_fsm::Pose>("current_pose", current_pose);
+    spdlog::debug("Current position: x = {}, y = {}, z = {}",
+                  feedback->feedback.x, feedback->feedback.y,
+                  feedback->feedback.z);
+}
+
+std::string AbortState(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) {
+    blackboard->set<bool>("is_abort", true);
+    return yasmin_ros::basic_outcomes::ABORT;
+};
+
+std::string ErrorState(
+    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) {
+    blackboard->set<bool>("is_error", true);
+    return yasmin_ros::basic_outcomes::SUCCEED;
+};
+
+std::shared_ptr<yasmin::StateMachine> create_state_machines() {
+    std::set<std::string> outcomes = {
+        "error",
+        yasmin_ros::basic_outcomes::SUCCEED,
+        yasmin_ros::basic_outcomes::CANCEL,
+        yasmin_ros::basic_outcomes::ABORT,
+        yasmin_ros::basic_outcomes::TIMEOUT,
+    };
+    return std::make_shared<yasmin::StateMachine>(outcomes);
+}
+
+void add_states(std::shared_ptr<yasmin::StateMachine> sm,
+                std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) {
+    sm->add_state(
+        "FIND_PIPELINE", std::make_shared<FindPipelineState>(blackboard),
+        {
+            {yasmin_ros::basic_outcomes::SUCCEED, "APPROACH_PIPELINE"},
+            {yasmin_ros::basic_outcomes::ABORT, "ABORT"},
+        });
+    sm->add_state("APPROACH_PIPELINE",
+                  std::make_shared<ApproachPipelineState>(blackboard),
+                  {
+                      {yasmin_ros::basic_outcomes::SUCCEED, "FOLLOW_PIPELINE"},
+                      {yasmin_ros::basic_outcomes::CANCEL, "FIND_PIPELINE"},
+                      {yasmin_ros::basic_outcomes::ABORT, "ABORT"},
+                  });
+
+    sm->add_state("FOLLOW_PIPELINE",
+                  std::make_shared<FollowPipelineState>(blackboard),
+                  {
+                      {yasmin_ros::basic_outcomes::SUCCEED, "RETURN_HOME"},
+                      {yasmin_ros::basic_outcomes::ABORT, "ABORT"},
+                      {yasmin_ros::basic_outcomes::CANCEL, "APPROACH_PIPELINE"},
+
+                  });
+
+    sm->add_state("RETURN_HOME", std::make_shared<ReturnHomeState>(blackboard),
+                  {
+                      {yasmin_ros::basic_outcomes::SUCCEED, "FIND_PIPELINE"},
+                      {yasmin_ros::basic_outcomes::CANCEL, "error"},
+                      {yasmin_ros::basic_outcomes::ABORT, "ABORT"},
+
+                  });
+    sm->add_state("ABORT",
+                  std::make_shared<yasmin::CbState>(
+                      std::initializer_list<std::string>{
+                          yasmin_ros::basic_outcomes::SUCCEED,
+                          yasmin_ros::basic_outcomes::CANCEL,
+                          yasmin_ros::basic_outcomes::ABORT},
+                      AbortState),
+                  {
+                      {yasmin_ros::basic_outcomes::SUCCEED,
+                       yasmin_ros::basic_outcomes::ABORT},
+                      {yasmin_ros::basic_outcomes::CANCEL,
+                       yasmin_ros::basic_outcomes::ABORT},
+                      {yasmin_ros::basic_outcomes::ABORT,
+                       yasmin_ros::basic_outcomes::ABORT},
+                  });
+    sm->add_state("ERROR",
+                  std::make_shared<yasmin::CbState>(
+                      std::initializer_list<std::string>{
+                          "error", yasmin_ros::basic_outcomes::SUCCEED,
+                          yasmin_ros::basic_outcomes::CANCEL,
+                          yasmin_ros::basic_outcomes::ABORT},
+                      ErrorState),
+                  {
+                      {yasmin_ros::basic_outcomes::SUCCEED, "error"},
+                      {yasmin_ros::basic_outcomes::CANCEL, "error"},
+                      {yasmin_ros::basic_outcomes::ABORT, "error"},
+                  });
+}
+
+auto initialize_blackboard() {
+    auto params = std::make_shared<rclcpp::Node>("pipeline_params");
+    spdlog::debug("Creating params node");
+
+    params->declare_parameter<double>("fsm.pipeline.pipeline_offset");
+    params->declare_parameter<int>("fsm.pipeline.num_measurements");
+
+    params->declare_parameter<std::string>("action_servers.reference_filter");
+    params->declare_parameter<std::string>("action_servers.los");
+    params->declare_parameter<std::string>("action_name");
+
+    spdlog::debug("Parameters declared");
+
+    auto blackboard = std::make_shared<yasmin::blackboard::Blackboard>();
+
+    pipeline_fsm::PoseStamped pipeline_start_pose;
+    pipeline_fsm::PoseStamped start_pose;
+    blackboard->set<pipeline_fsm::PoseStamped>("pipeline_start_pose",
+                                               pipeline_start_pose);
+    blackboard->set<pipeline_fsm::PoseStamped>("start_pose", start_pose);
+    blackboard->set<bool>("return_home", false);
+    blackboard->set<bool>("is_home", true);
+    blackboard->set<bool>("is_finished", false);
+    blackboard->set<bool>("is_error", false);
+    blackboard->set<bool>("has_finished_converging", false);
+    blackboard->set<int>(
+        "num_measurements",
+        params->get_parameter("fsm.pipeline.num_measurements").as_int());
+    blackboard->set<std::string>(
+        "reference_filter_action",
+        params->get_parameter("action_servers.reference_filter").as_string());
+    blackboard->set<std::string>(
+        "los_guidance_action",
+        params->get_parameter("action_servers.los").as_string());
+    blackboard->set<std::string>(
+        "pose_action", params->get_parameter("action_name").as_string());
+    blackboard->set<double>(
+        "pipeline_offset",
+        params->get_parameter("fsm.pipeline.pipeline_offset").as_double());
+    blackboard->set<pipeline_fsm::PoseStamped>("pipeline_offset_goal",
+                                               pipeline_start_pose);
+    spdlog::debug("Blackboard created");
+
+    return blackboard;
+}
+
+int main(int argc, char* argv[]) {
+    spdlog::info("Pipeline");
+    rclcpp::init(argc, argv);
+
+    yasmin_ros::set_ros_loggers();
+
+    std::shared_ptr<yasmin::StateMachine> sm = create_state_machines();
+
+    rclcpp::on_shutdown([sm]() {
+        if (sm->is_running()) {
+            sm->cancel_state();
+        }
+    });
+
+    auto blackboard = initialize_blackboard();
+
+    add_states(sm, blackboard);
+
+    yasmin_viewer::YasminViewerPub yasmin_pub("Pipeline", sm);
+
+    spdlog::debug("State machines created");
+
+    try {
+        std::string outcome = (*sm.get())(blackboard);
+        spdlog::info("State machine finished with outcome: {}", outcome);
+    } catch (const std::exception& e) {
+        spdlog::warn(e.what());
+        rcutils_reset_error();
+    }
+
+    if (!rclcpp::ok()) {
+        spdlog::info(
+            "ROS2 context is already invalid. Skipping publisher destruction.");
+        return 1;
+    }
+
+    if (rclcpp::ok()) {
+        sm.reset();
+        blackboard.reset();
+
+        rclcpp::shutdown();
+        spdlog::info("ROS2 shutdown completed gracefully.");
+    }
+
+    return 0;
+}