Move/pipeline fsm

mission/tacc/pipeline_inspection/README.md

@@ -0,0 +1,98 @@
+# Pipeline Inspection FSM
+
+A ROS2 finite state machine (FSM) that enables an AUV to autonomously search for, detect, and follow a subsea pipeline. Built with [YASMIN](https://github.com/uleroboticsgroup/yasmin) for state management.
+
+## Overview
+
+The FSM orchestrates a multi-stage pipeline inspection mission:
+
+1. Waits for an external start signal
+2. Executes a search pattern while polling for pipeline landmarks
+3. Converges to the detected pipeline start
+4. Hands off to pipeline following
+5. Waits for an end-of-pipeline signal, then terminates cleanly
+
+## State Machine
+
+```
+WAIT_FOR_START
+    │ (start_mission service called)
+    ▼
+SEARCH ─── SearchPatternState (navigate through search waypoints)
+    │       LandmarkPollingState (concurrent landmark detection)
+    │ (landmark found)
+    ▼
+CONVERGE
+    │ (AUV at pipeline start)
+    ▼
+START_PIPELINE_TRG
+    │ (start_pipeline_following service called)
+    ▼
+START_WM
+    │ (persistent WaypointManager goal sent)
+    ▼
+PIPELINE_FOLLOWING
+    │ (pipeline_finished service called)
+    ▼
+STOP_WM
+    │
+    ▼
+DONE
+```
+
+## ROS2 Interface
+
+### Services Provided
+
+| Service | Type | Description |
+|---|---|---|
+| `pipeline_inspection_fsm/start_mission` | `std_srvs/Trigger` | Starts the mission |
+| `pipeline_inspection_fsm/start_pipeline_following` | `std_srvs/Trigger` | Signals the FSM to begin persistent pipeline following |
+| `pipeline_inspection_fsm/pipeline_finished` | `std_srvs/Trigger` | Signals the end of the pipeline |
+
+### Action Clients
+
+| Action Server | Type | Description |
+|---|---|---|
+| `waypoint_manager` | `vortex_msgs/action/WaypointManager` | Navigation along waypoints |
+| `landmark_polling` | `vortex_msgs/action/LandmarkPolling` | Pipeline landmark detection |
+
+## Configuration
+
+### `search_waypoints.yaml`
+
+Defines the search pattern the AUV follows while looking for the pipeline:
+
+```yaml
+search_waypoint_1:
+  position: {x: 2.0, y: 0.0, z: 0.0}
+  orientation: {roll: 0.0, pitch: 0.0, yaw: 0.0}
+  mode: 0
+  convergence_threshold: 0.1
+```
+
+Add additional `search_waypoint_N` entries to extend the search area.
+
+### `pipeline_convergence.yaml`
+
+Defines a pose offset applied to the detected landmark position when converging:
+
+```yaml
+pipeline_start_convergence:
+  position: {x: 0.0, y: 0.0, z: -1.0}
+  orientation: {roll: 0.0, pitch: 0.0, yaw: 0.0}
+  convergence_threshold: 0.1
+  dead_reckoning_threshold: 0.5
+```
+## Triggering the Mission
+
+```bash
+# 1. Start the mission
+ros2 service call /orca/pipeline_inspection_fsm/start_mission std_srvs/srv/Trigger
+
+# 2. (Called internally by the FSM after convergence)
+ros2 service call /orca/pipeline_inspection_fsm/start_pipeline_following std_srvs/srv/Trigger
+
+# 3. Signal end of pipeline when following is complete
+ros2 service call /orca/pipeline_inspection_fsm/pipeline_finished std_srvs/srv/Trigger
+```

mission/tacc/pipeline_inspection/pipeline_inspection_fsm/CMakeLists.txt

@@ -0,0 +1,74 @@
+cmake_minimum_required(VERSION 3.8)
+project(pipeline_inspection_fsm)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 20)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_index_cpp REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(yasmin REQUIRED)
+find_package(yasmin_ros REQUIRED)
+find_package(yasmin_viewer REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(vortex_utils REQUIRED)
+find_package(vortex_utils_ros REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+include_directories(include)
+
+add_executable(pipeline_inspection_fsm
+  src/trigger_wait_state.cpp
+  src/wait_for_start_state.cpp
+  src/search_pattern_state.cpp
+  src/landmark_polling_state.cpp
+  src/converge_state.cpp
+  src/start_waypoint_manager_state.cpp
+  src/wait_for_pipeline_end_state.cpp
+  src/stop_waypoint_manager_state.cpp
+  src/blackboard.cpp
+  src/fsm_runner.cpp
+)
+
+ament_target_dependencies(pipeline_inspection_fsm
+  ament_index_cpp
+  rclcpp
+  rclcpp_action
+  std_srvs
+  yasmin
+  yasmin_ros
+  yasmin_viewer
+  vortex_msgs
+  vortex_utils
+  vortex_utils_ros
+  geometry_msgs
+)
+
+install(TARGETS
+  pipeline_inspection_fsm
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

mission/tacc/pipeline_inspection/pipeline_inspection_fsm/config/pipeline_convergence.yaml

@@ -0,0 +1,12 @@
+pipeline_start_convergence:
+  position:
+    x: 0.0
+    y: 0.0
+    z: -1.0
+  orientation:
+    roll: 0.0
+    pitch: 0.0
+    yaw: 0.0
+  mode: 0
+  convergence_threshold: 0.1
+  dead_reckoning_threshold: 0.5

mission/tacc/pipeline_inspection/pipeline_inspection_fsm/config/pipeline_inspection_fsm_config.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    services:
+      start_pipeline_following: "pipeline_inspection_fsm/start_pipeline_following"
+      end_of_pipeline: "pipeline_inspection_fsm/pipeline_finished"

mission/tacc/pipeline_inspection/pipeline_inspection_fsm/config/search_waypoints.yaml

@@ -0,0 +1,48 @@
+
+search_waypoint_1:
+  position:
+    x: 2.0
+    y: 0.0
+    z: 0.0
+  orientation:
+    roll: 0.0
+    pitch: 0.0
+    yaw: 0.0
+  mode: 0
+  convergence_threshold: 0.1
+
+search_waypoint_2:
+  position:
+    x: 4.0
+    y: 0.0
+    z: 0.0
+  orientation:
+    roll: 0.0
+    pitch: 0.0
+    yaw: 0.0
+  mode: 0
+  convergence_threshold: 0.1
+
+search_waypoint_3:
+  position:
+    x: 4.0
+    y: -2.0
+    z: 0.0
+  orientation:
+    roll: 0.0
+    pitch: 0.0
+    yaw: 0.0
+  mode: 0
+  convergence_threshold: 0.1
+
+search_waypoint_4:
+  position:
+    x: 2.0
+    y: -2.0
+    z: 0.0
+  orientation:
+    roll: 0.0
+    pitch: 0.0
+    yaw: 0.0
+  mode: 0
+  convergence_threshold: 0.1