fill landmark polygons

asv_setup/config/sitaw/map_manager_params.yaml

@@ -16,6 +16,10 @@ map_manager_node:
     # map_origin_lon: 150.6740063854522
     # use_predef_map_origin: true
     # Map origin for sim_maneuvering
-    map_origin_lat: -33.72213985487166
-    map_origin_lon: 150.67413507552098
+    # map_origin_lat: -33.72213985487166
+    # map_origin_lon: 150.67413507552098
+    # use_predef_map_origin: true
+    #colav
+    map_origin_lat: -33.72213988382845
+    map_origin_lon: 150.67413500672993
     use_predef_map_origin: true

mission/landmark_server/include/landmark_server/grid_manager.hpp

@@ -26,6 +26,13 @@ class GridManager {
 
   void draw_line(int x0, int y0, int x1, int y1, int8_t *grid, int value);
 
+  void fill_polygon(int8_t *grid,
+                               const Eigen::Array<float, 2, Eigen::Dynamic> &polygon,
+                               int value);
+
+  bool point_in_polygon(int x, int y, const Eigen::Array<float, 2, Eigen::Dynamic> &polygon);
+
+
 private:
   float resolution_;
   uint32_t height_;

mission/landmark_server/src/grid_manager.cpp

@@ -25,6 +25,40 @@ void GridManager::handle_polygon(
 
     draw_line(x0, y0, x1, y1, grid, value);
   }
+  if ( vertices.cols() > 1){
+  fill_polygon(grid, vertices, value);
+  }
+}
+
+void GridManager::fill_polygon(int8_t *grid,
+                               const Eigen::Array<float, 2, Eigen::Dynamic> &polygon,
+                               int value) {
+      double max_x = polygon.row(0).maxCoeff();
+      double min_x = polygon.row(0).minCoeff();
+      double max_y = polygon.row(1).maxCoeff();
+      double min_y = polygon.row(1).minCoeff();
+
+      for (int x = min_x; x < max_x; x++) {
+          for (int y = min_y; y < max_y; y++) {
+            if (x >= 0 && x < static_cast<int>(width_) && y >= 0 && y < static_cast<int>(height_)) {
+                if (point_in_polygon(x, y, polygon)) {
+                    grid[y * width_ + x] += value;
+                }
+          }
+        }  
+      }
+}
+
+bool GridManager::point_in_polygon(int x, int y, const Eigen::Array<float, 2, Eigen::Dynamic> &polygon) {
+    int i, j;
+    bool c = false;
+    for (i = 0, j = polygon.cols() - 1; i < polygon.cols(); j = i++) {
+        if (((polygon(1, i) > y) != (polygon(1, j) > y)) &&
+            (x < (polygon(0, j) - polygon(0, i)) * (y - polygon(1, i)) / (polygon(1, j) - polygon(1, i)) + polygon(0, i))
+            )
+            c = !c;
+    }
+    return c;
 }
 
 void GridManager::draw_line(int x0, int y0, int x1, int y1, int8_t *grid,

mission/map_manager/params/map_manager_params.yaml

@@ -7,9 +7,9 @@ map_manager_node:
     map_height: 1000
     frame_id: "world"
     # Map origin for office rosbag
-    # map_origin_lat: 63.414660884931976 
-    # map_origin_lon: 10.398554661537544
-    # use_predef_map_origin: true
-    map_origin_lat: 0.0
-    map_origin_lon: 0.0
-    use_predef_map_origin: false
+    map_origin_lat: 63.414660884931976 
+    map_origin_lon: 10.398554661537544
+    use_predef_map_origin: true
+    # map_origin_lat: 0.0
+    # map_origin_lon: 0.0
+    # use_predef_map_origin: false

mission/map_manager/src/map_manager_ros.cpp

@@ -213,7 +213,7 @@ nav_msgs::msg::OccupancyGrid MapManagerNode::createOccupancyGrid() {
   grid.info.resolution = get_parameter("map_resolution").as_double();
   grid.info.width = get_parameter("map_width").as_int();
   grid.info.height = get_parameter("map_height").as_int();
-  // grid.info.origin = calculate_map_origin();
+  grid.info.origin = calculate_map_origin();
   grid.info.map_load_time = this->now();
   // 0 represents unoccupied, 1 represents definitely occupied, and
   // -1 represents unknown.

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

@@ -57,6 +57,22 @@ class DockingTaskNode : public NjordTaskBaseNode {
 
   void initialize_grid_sub();
 
+  void find_dock_structure_edges();
+
+  /**
+   * @brief Iterate over line in grid map. Returns a vector of booleans
+   * 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.
+   */
+  std::vector<bool> search_line(const nav_msgs::msg::OccupancyGrid &grid, double x0, double y0, double x1, double y1);
+
+
 private:
   mutable std::mutex grid_mutex_;
   bool new_grid_msg_ = false;

mission/njord_tasks/docking_task/params/docking_task_params.yaml

@@ -23,7 +23,14 @@ docking_task_node:
     distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame
     # 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: "grid"
+    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_width: 4.13 # width of available docking space
     dock_width_tolerance: 0.5
     dock_length: 2.0  # length of available docking space

mission/njord_tasks/docking_task/src/docking_task_ros.cpp

@@ -247,15 +247,24 @@ void DockingTaskNode::main_task() {
   buoy_vis_msg.header.stamp = this->now();
   buoy_visualization_pub_->publish(buoy_vis_msg);
 
+  Eigen::Vector2d direction_vector_up;
+  direction_vector_up << (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x +
+       buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) /
+      2 - odom_start_point_.x,
+      (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y +
+       buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) /
+      2 - odom_start_point_.y;
+  direction_vector_up.normalize();
+
   geometry_msgs::msg::Point waypoint_third_pair;
   waypoint_third_pair.x =
       (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x +
        buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) /
-      2;
+      2 + direction_vector_up(0) * 2;
   waypoint_third_pair.y =
       (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y +
        buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) /
-      2;
+      2 + direction_vector_up(1) * 2;
   waypoint_third_pair.z = 0.0;
   send_waypoint(waypoint_third_pair);
   set_desired_heading(odom_start_point_, waypoint_third_pair);
@@ -377,4 +386,63 @@ std::shared_ptr<nav_msgs::msg::OccupancyGrid> DockingTaskNode::get_grid() {
   return grid_msg_;
 }
 
+std::vector<bool> DockingTaskNode::search_line(const nav_msgs::msg::OccupancyGrid &grid, double dx0, double dy0,
+                                               double dx1, double dy1) {
+  int x0 = x0 / grid.info.resolution + grid.info.width / 2;
+  int y0 = y0 / grid.info.resolution + grid.info.height / 2;
+  int x1 = x1 / grid.info.resolution + grid.info.width / 2;
+  int y1 = y1 / grid.info.resolution + grid.info.height / 2;
+
+  std::vector<bool> occupied_cells;
+  bool steep = std::abs(y1 - y0) > std::abs(x1 - x0);
+  if (steep) {
+    occupied_cells.reserve(std::abs(y1 - y0));
+  } else {
+    occupied_cells.reserve(std::abs(x1 - x0));
+  }
+  if (steep) {
+    std::swap(x0, y0);
+    std::swap(x1, y1);
+  }
+  bool swap = x0 > x1;
+  if (swap) {
+    std::swap(x0, x1);
+    std::swap(y0, y1);
+  }
+  int dx = x1 - x0;
+  int dy = std::abs(y1 - y0);
+  int error = 2 * dy - dx;
+  int ystep = (y0 < y1) ? 1 : -1;
+  int xbounds = steep ? grid.info.height : grid.info.width;
+  int ybounds = steep ? grid.info.width : grid.info.height;
+  int y = y0;
+
+  for (int x = x0; x <= x1; x++) {
+    if (steep) {
+      if (x >= 0 && x < xbounds && y >= 0 && y < ybounds) {
+        occupied_cells.push_back(grid.data[y + x * grid.info.width] > 0);
+      }
+    } else {
+      if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) {
+        occupied_cells.push_back(grid.data[y * grid.info.width + x] > 0);
+      }
+    }
+    if (error > 0) {
+      y += ystep;
+      error -= 2 * (dx - dy);
+    } else {
+      error += 2 * dy;
+    }
+  }
+  if (swap) {
+    std::reverse(occupied_cells.begin(), occupied_cells.end());
+  }
+  return occupied_cells;
+
+}
+
+void DockingTaskNode::find_dock_structure_edges(){
+
+}
+
 } // namespace docking_task