dock edge localization test

mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp

@@ -57,21 +57,22 @@ class DockingTaskNode : public NjordTaskBaseNode {
 
   void initialize_grid_sub();
 
-  void find_dock_structure_edges();
+  std::pair<Eigen::Vector2d, Eigen::Vector2d> find_dock_structure_edges(const geometry_msgs::msg::Point &waypoint_third_pair,
+  Eigen::Vector2d &direction_vector_up);
 
   /**
-   * @brief Iterate over line in grid map. Returns a vector of booleans
+   * @brief Iterate over line in grid map. Returns a vector
    * representing occupied cells along the line.
    *
    * @param grid The grid map.
    * @param x0 The x-coordinate of the start point.
    * @param y0 The y-coordinate of the start point.
    * @param x1 The x-coordinate of the end point.
    * @param y1 The y-coordinate of the end point.
-   * @return A vector of booleans representing occupied cells along the line.
+   * @return A vector representing occupied cells along the line.
    */
-  std::vector<bool> search_line(const nav_msgs::msg::OccupancyGrid &grid,
-                                double x0, double y0, double x1, double y1);
+  std::vector<int8_t> search_line(const nav_msgs::msg::OccupancyGrid &grid, double x0, double y0, double x1, double y1);
+
 
 private:
   mutable std::mutex grid_mutex_;

mission/njord_tasks/docking_task/params/docking_task_params.yaml

@@ -24,9 +24,12 @@ docking_task_node:
     # If the distance is greater than 6.0 we drive distance-4m towards the first buoy pair before trying to find it
     distance_between_buoys_in_pair: 5.0 # Distance between the two buoys in a pair
     grid_topic: "pcl_grid"
-    dock_structure_absolute_width: 4.39 # Width of the dock structure in meters 4.39 for task 4.2 and 4.52 for task 4.1
-    dock_structure_absolute_width_padding: 0.5 # Padding on each side of the dock structure for line search start and end
-
+    dock_structure_absolute_width: 4.39 # Width of the dock structure in meters 4.39
+    dock_structure_absolute_width_tolerance: 0.5 # Padding on each side of the dock structure for line search start and end
+    dock_edge_width: 0.13   # width of the dock edge
+    line_search_distance: 2.0 # distance to iterate along line when searching for dock edge
+    dock_search_offset: 2.0   # offset in forward direction from third buoy pair to begin search for dock
+
 
 
 
@@ -35,9 +38,7 @@ docking_task_node:
     dock_width_tolerance: 0.5
     dock_length: 2.0  # length of available docking space
     dock_length_tolerance: 0.5
-    dock_edge_width: 0.13   # width of the dock edge
     dock_edge_width_tolerance: 0.05
-    dock_search_offset: 0.2   # offset in forward direction to begin search for dock
     task_nr: 2 # possible values 1 and 2 corresponding to task 4.1 and 4.2
     models:
       dynmod_stddev: 0.01