Wall update

Added functionality to map walls into the grid. Wall width mapped to grid is customizable through param in landmark server

asv_setup/config/sitaw/land_polygon_office.yaml

@@ -0,0 +1,16 @@
+63.41489120183045 10.39705552070149
+63.41497265487507 10.39783732516908
+63.41538547016246 10.397651181248118
+63.415431749532054 10.398250978326427
+63.415048554100395 10.398449531842004
+63.41512075073269 10.399185834462275
+63.41495229164123 10.399305793877936
+63.41486158248972 10.398540535536645
+63.41453021396673 10.398747362115373
+63.4145246602717 10.398631539231284
+63.414280296625655 10.39877218130482
+63.41424142039905 10.398197203415958
+63.4144598670836 10.398110336252891
+63.41444505719146 10.397990376837232
+63.41478938520484 10.397841461700548
+63.41472089017681 10.397167207053899

asv_setup/config/sitaw/landmark_server_params.yaml

@@ -1,4 +1,5 @@
 landmark_server_node:
   ros__parameters:
     fixed_frame: map
-    target_frame: base_link
+    target_frame: base_link
+    wall_width: 0.1

asv_setup/config/sitaw/map_manager_params.yaml

@@ -1,7 +1,7 @@
 map_manager_node:
   ros__parameters:
     use_predef_landmask: true
-    landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon.yaml"
+    landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_office.yaml"
     map_resolution: 0.2
     map_width: 1000
     map_height: 1000

mission/landmark_server/include/landmark_server/grid_manager.hpp

@@ -5,74 +5,28 @@
 
 namespace landmark_server {
 
-enum class ShapeType : uint8_t { SPHERE = 0, PRISM = 1 };
-
-struct Circle {
-  float radius;
-  float x_centre;
-  float y_centre;
-};
-
-struct Point {
-  float x;
-  float y;
-  float z;
-};
-
-struct Polygon {
-  std::vector<Point> vertices;
-};
-
-struct Pose {
-  float x;
-  float y;
-  float yaw;
-};
-
-struct ShapeInfo {
-  ShapeType type;
-  Circle circle;
-  Polygon polygon;
-  Pose pose;
-};
-
+/**
+ * @class GridManager
+ * @brief A class for managing the grid map.
+ */
 class GridManager {
 
 public:
-  using Grid = Eigen::Array<int8_t, Eigen::Dynamic, Eigen::Dynamic>;
-
-  // GridManager(std::vector<int8_t> grid, float resolution, uint32_t height,
-  // uint32_t width);
   GridManager(float resolution, uint32_t height, uint32_t width);
-  ~GridManager() = default;
 
-  const Grid &get_grid() const;
+  ~GridManager() = default;
 
   void update_grid(int8_t *grid,
                    const Eigen::Array<float, 2, Eigen::Dynamic> &polygon,
                    int value);
 
-  std::tuple<int, int> world_to_grid(float x, float y);
-
-  void handle_circle(int xc, int yc, int r, int value);
-
-  void draw_circle(
-      int xc, int yc, int x, int y, int value,
-      Eigen::Block<landmark_server::GridManager::Grid, -1, -1, false> &block,
-      int xmin, int ymin);
-
   void handle_polygon(int8_t *grid,
                       const Eigen::Array<float, 2, Eigen::Dynamic> &vertices,
                       int value);
 
-  void draw_polygon(
-      int x0, int y0, int x1, int y1,
-      Eigen::Block<landmark_server::GridManager::Grid, -1, -1, false> &block,
-      int value);
   void draw_line(int x0, int y0, int x1, int y1, int8_t *grid, int value);
 
 private:
-  Grid grid_;
   float resolution_;
   uint32_t height_;
   uint32_t width_;

mission/landmark_server/include/landmark_server/landmark_server.hpp

@@ -71,7 +71,7 @@ class LandmarkServerNode : public rclcpp::Node {
       const vortex_msgs::msg::LandmarkArray::SharedPtr msg);
 
   Eigen::Array<float, 2, Eigen::Dynamic>
-  get_convex_hull(const shape_msgs::msg::SolidPrimitive &solid_primitive);
+  get_convex_hull(const vortex_msgs::msg::Landmark& landmark);
 
   void get_grid();
 

mission/landmark_server/params/landmark_server_params.yaml

@@ -1,4 +1,5 @@
 landmark_server_node:
   ros__parameters:
     fixed_frame: world
-    target_frame: base_link
+    target_frame: base_link
+    wall_width: 0.1

mission/landmark_server/src/grid_manager.cpp

@@ -3,113 +3,22 @@
 
 namespace landmark_server {
 
-// GridManager::GridManager(std::vector<int8_t> grid, float resolution, uint32_t
-// height, uint32_t width)
-//     : resolution_(resolution), height_(height), width_(width)
-// {
-
-//     if (grid.size() != height * width) {
-//         throw std::runtime_error("Grid size does not match height * width");
-//     }
-//     grid_ = Eigen::Map<const Eigen::Array<int8_t, Eigen::Dynamic,
-//     Eigen::Dynamic>>
-//         (grid.data(), height, width);
-// }
 
 GridManager::GridManager(float resolution, uint32_t height, uint32_t width)
     : resolution_(resolution), height_(height), width_(width) {}
 
-const GridManager::Grid &GridManager::get_grid() const { return grid_; }
-
 void GridManager::update_grid(
     int8_t *grid, const Eigen::Array<float, 2, Eigen::Dynamic> &polygon,
     int value) {
   handle_polygon(grid, polygon, value);
 }
 
-// void GridManager::update_grid(const ShapeInfo& shape_info, int value)
-// {
-//     switch (shape_info.type)
-//     {
-//     case ShapeType::SPHERE:
-//         handle_circle(shape_info.circle.x_centre, shape_info.circle.y_centre,
-//         shape_info.circle.radius, value); break;
-//     case ShapeType::PRISM:
-//         handle_polygon(shape_info.polygon.vertices, value);
-//     default:
-//         break;
-//     }
-// }
-
-void GridManager::draw_circle(
-    int xc, int yc, int x, int y, int value,
-    Eigen::Block<landmark_server::GridManager::Grid, -1, -1, false> &block,
-    int xmin, int ymin) {
-  // Translate coordinates to block local coordinates
-  int bx = xc - xmin;
-  int by = yc - ymin;
-
-  if (bx + x < block.rows() && by + y < block.cols() && bx + x >= 0 &&
-      by + y >= 0) {
-    block(bx + x, by + y) = value;
-    block(bx - x, by + y) = value;
-    block(bx + x, by - y) = value;
-    block(bx - x, by - y) = value;
-    block(bx + y, by + x) = value;
-    block(bx - y, by + x) = value;
-    block(bx + y, by - x) = value;
-    block(bx - y, by - x) = value;
-  }
-}
-
-void GridManager::handle_circle(int xc, int yc, int r, int value) {
-  int xmin = static_cast<int>((xc - r) / resolution_ + width_ / 2);
-  int ymin = static_cast<int>((yc - r) / resolution_ + width_ / 2);
-  int xmax = static_cast<int>((xc + r) / resolution_ + width_ / 2);
-  int ymax = static_cast<int>((yc + r) / resolution_ + width_ / 2);
-
-  if (xmin < 0 || ymin < 0 || xmax >= grid_.rows() || ymax >= grid_.cols()) {
-    throw std::runtime_error("Circle exceeds grid boundaries");
-  }
-
-  auto block = grid_.block(xmin, ymin, xmax - xmin + 1, ymax - ymin + 1);
-  int x = 0, y = r;
-  int d = 3 - 2 * r;
-  draw_circle(xc, yc, x, y, value, block, xmin, ymin);
-  while (y >= x) {
-    x++;
-    if (d > 0) {
-      y--;
-      d = d + 4 * (x - y) + 10;
-    } else {
-      d = d + 4 * x + 6;
-    }
-    draw_circle(xc, yc, x, y, value, block, xmin, ymin);
-  }
-}
-
 void GridManager::handle_polygon(
     int8_t *grid, const Eigen::Array<float, 2, Eigen::Dynamic> &vertices,
     int value) {
-  // Determine the bounding box of the polygon
-  // float min_x = vertices.row(0).minCoeff();
-  // float min_y = vertices.row(1).minCoeff();
-  // float max_x = vertices.row(0).maxCoeff();
-  // float max_y = vertices.row(1).maxCoeff();
-
-  // int xmin = static_cast<int>(min_x / resolution_ + width_ / 2);
-  // int ymin = static_cast<int>(min_y / resolution_ + height_ / 2);
-  // int xmax = static_cast<int>(max_x / resolution_ + width_ / 2);
-  // int ymax = static_cast<int>(max_y / resolution_ + height_ / 2);
-
-  // if (xmin < 0 || ymin < 0 || xmax >= height_ || ymax >= width_) {
-  //     throw std::runtime_error("Polygon exceeds grid boundaries");
-  // }
-
-  // Draw the polygon
+  // Convert to grid coordinates
   for (Eigen::Index i = 0; i < vertices.cols(); ++i) {
     Eigen::Index j = (i + 1) % vertices.cols();
-    std::cout << "i: " << i << " j: " << j << std::endl;
     int x0 = static_cast<int>(vertices(0, i) / resolution_ + width_ / 2);
     int y0 = static_cast<int>(vertices(1, i) / resolution_ + height_ / 2);
     int x1 = static_cast<int>(vertices(0, j) / resolution_ + width_ / 2);
@@ -119,46 +28,6 @@ void GridManager::handle_polygon(
   }
 }
 
-void GridManager::draw_polygon(
-    int x0, int y0, int x1, int y1,
-    Eigen::Block<landmark_server::GridManager::Grid, -1, -1, false> &block,
-    int value) {
-  bool steep = std::abs(y1 - y0) > std::abs(x1 - x0);
-  if (steep) {
-    std::swap(x0, y0);
-    std::swap(x1, y1);
-  }
-  if (x0 > x1) {
-    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 ? block.rows() : block.cols();
-  int ybounds = steep ? block.cols() : block.rows();
-  int y = y0;
-
-  for (int x = x0; x <= x1; x++) {
-    if (steep) {
-      if (x >= 0 && x < xbounds && y >= 0 && y < ybounds) {
-        block(y, x) += value;
-      }
-    } else {
-      if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) {
-        block(x, y) += value;
-      }
-    }
-    if (error > 0) {
-      y += ystep;
-      error -= 2 * (dx - dy);
-    } else {
-      error += 2 * dy;
-    }
-  }
-}
-
 void GridManager::draw_line(int x0, int y0, int x1, int y1, int8_t *grid,
                             int value) {
   bool steep = std::abs(y1 - y0) > std::abs(x1 - x0);