Dev/subsea docking fsm

.pre-commit-config.yaml

@@ -113,7 +113,7 @@ repos:
     rev: v2.4.1
     hooks:
       - id: codespell
-        args: ['--write-changes', '--ignore-words-list=theses,fom,NED,ENU']
+        args: ['--write-changes', '--ignore-words-list=theses,fom,NED,ENU,Alle']
 
 ci:
     autoupdate_schedule: quarterly

mission/tacc/subsea_docking/docking_position_estimator/CMakeLists.txt

@@ -0,0 +1,90 @@
+cmake_minimum_required(VERSION 3.8)
+project(docking_position_estimator)
+
+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 dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(fmt REQUIRED)
+find_package(spdlog REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(vortex_utils REQUIRED)
+find_package(vortex_utils_ros REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(visualization_msgs REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+include_directories(include)
+
+set(LIB_NAME ${PROJECT_NAME}_lib)
+add_library(${LIB_NAME} SHARED
+  src/docking_position_estimator.cpp
+  src/docking_position_estimator_ros.cpp
+)
+
+ament_target_dependencies(${LIB_NAME} PUBLIC
+  rclcpp
+  rclcpp_components
+  rclcpp_action
+  tf2_geometry_msgs
+  tf2_eigen
+  geometry_msgs
+  vortex_msgs
+  nav_msgs
+  vortex_utils
+  vortex_utils_ros
+  Eigen3
+  spdlog
+  fmt
+  message_filters
+  #for testing:
+  visualization_msgs
+)
+
+rclcpp_components_register_node(
+  ${LIB_NAME}
+  PLUGIN "vortex::docking_position_estimator::DockingPositionEstimatorNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+ament_export_targets(export_${LIB_NAME})
+
+install(TARGETS ${LIB_NAME}
+  EXPORT export_${LIB_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+if(BUILD_TESTING)
+  add_subdirectory(test)
+endif()
+
+ament_package()

mission/tacc/subsea_docking/docking_position_estimator/config/docking_position_estimator.yaml

@@ -0,0 +1,36 @@
+/**:
+  ros__parameters:
+    # Topics
+    line_sub_topic: "/line_detection/line_segments"
+    pose_sub_topic: "/pose"
+    sonar_info_sub_topic: "/moby/fls/sonar_info"
+    debug_topic: "/debug"
+
+    # Publish
+    # publish_rate_ms: 200 #not used?
+
+    # Frames / TF
+    odom_frame: "moby/odom"
+    # base_frame: "moby/base_link" # not used
+    # sonar_frame: "moby/sonar_link" # not used
+
+    # Corner/docking params
+    min_wall_distance_m: 0.5
+    max_wall_distance_m: 10.0
+
+    parallel_heading_angle_threshold_rad: 0.4
+    perpendicular_heading_angle_threshold_rad: 1.35
+    min_corner_angle_rad: 1.50
+    max_corner_angle_rad: 1.70
+    side_wall_offset_m: 2.5 #offset along right wall og left wall
+    far_wall_offset_m: 4.0 #offset along far wall
+
+    right_wall_max_y_m: 0.4
+    far_wall_min_x_m: 0.5
+
+    use_left_wall: false
+
+    # Waypoint (not used now)
+    switching_threshold: 0.5
+    overwrite_prior_waypoints: true
+    take_priority: true