630 task pool grid exploration

filtering/line_filtering/config/line_filtering.yaml

@@ -1,12 +1,12 @@
 /**:
   ros__parameters:
     line_sub_topic: "/line_detection/line_segments"
-    sonar_info_sub_topic: "/orca/sonar_info"
+    sonar_info_sub_topic: "/moby/fls/sonar_info"
     line_pub_topic: "/filtered_lines"
     marker_pub_topic: "/filtered_lines_markers"
 
-    target_frame: "odom"
-    sonar_frame: "Orca/FLS"
+    target_frame: "moby/odom"
+    sonar_frame: "moby/sonar_link"
 
     timer_rate_ms: 200
 

mission/tacc/docking_task/pool_exploration/CMakeLists.txt

@@ -0,0 +1,96 @@
+cmake_minimum_required(VERSION 3.8)
+project(pool_exploration)
+
+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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+include_directories(include)
+
+install(DIRECTORY
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+set(LIB_NAME ${PROJECT_NAME}_lib)
+add_library(${LIB_NAME} SHARED
+  src/pool_exploration.cpp
+  src/pool_exploration_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 "PoolExplorationNode"
+  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/docking_task/pool_exploration/README.md

@@ -0,0 +1,6 @@
+colcon build --packages-up-to pool_exploration --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=1
+
+
+Utvide til venstre hjørne også 
+
+Evt filtrere ut doble linjer tidligere

mission/tacc/docking_task/pool_exploration/config/pool_exploration.yaml

@@ -0,0 +1,55 @@
+/**:
+  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
+
+    # Frames / TF
+    odom_frame: "moby/odom"
+    base_frame: "moby/base_link"
+    sonar_frame: "moby/sonar_link"
+
+    # Corner/docking params
+    min_wall_distance_m: 0.5
+    max_wall_distance_m: 10.0
+
+    far_wall_heading_angle_threshold: 1.6 # gammel?
+    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
+    right_wall_offset_m: 2.5 #flytter langs høyre veggen
+    far_wall_offset_m: 4.0 #flytter langs far wall
+
+    right_wall_max_y_m: 0.4
+    far_wall_min_x_m: 0.5
+
+    right_dist: -0.1 # ENDRE NAVN ?
+    choose_right_corner: 1
+
+    # Waypoint
+    switching_threshold: 0.5
+    overwrite_prior_waypoints: true
+    take_priority: true
+
+# FOR MAP LOGIC
+    map_pub_topic: "/map" 
+    map_frame: "map"
+
+    # Map / grid
+    size_x: 1000.0
+    size_y: 1000.0
+    resolution: 0.1
+    frame_id: "map"
+
+    # Map cells
+    unknown_cell: -1
+    occupied_cell: 100
+
+    enu_to_ned: false