diff --git a/asv_setup/config/sitaw/land_polygon.yaml b/asv_setup/config/sitaw/land_polygon_grillstad.yaml similarity index 100% rename from asv_setup/config/sitaw/land_polygon.yaml rename to asv_setup/config/sitaw/land_polygon_grillstad.yaml diff --git a/asv_setup/config/sitaw/land_polygon_office.yaml b/asv_setup/config/sitaw/land_polygon_office.yaml new file mode 100644 index 0000000..90a4898 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/asv_setup/config/sitaw/landmark_server_params.yaml b/asv_setup/config/sitaw/landmark_server_params.yaml index 52e3ba3..9200273 100644 --- a/asv_setup/config/sitaw/landmark_server_params.yaml +++ b/asv_setup/config/sitaw/landmark_server_params.yaml @@ -1,4 +1,5 @@ landmark_server_node: ros__parameters: fixed_frame: map - target_frame: base_link \ No newline at end of file + target_frame: base_link + wall_width: 0.1 \ No newline at end of file diff --git a/asv_setup/config/sitaw/map_manager_params.yaml b/asv_setup/config/sitaw/map_manager_params.yaml index e2c4828..1fa57cf 100644 --- a/asv_setup/config/sitaw/map_manager_params.yaml +++ b/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 diff --git a/mission/landmark_server/include/landmark_server/grid_manager.hpp b/mission/landmark_server/include/landmark_server/grid_manager.hpp index 78f0970..31ab72c 100644 --- a/mission/landmark_server/include/landmark_server/grid_manager.hpp +++ b/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 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; - - // GridManager(std::vector 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 &polygon, int value); - std::tuple 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 &block, - int xmin, int ymin); - void handle_polygon(int8_t *grid, const Eigen::Array &vertices, int value); - void draw_polygon( - int x0, int y0, int x1, int y1, - Eigen::Block &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_; diff --git a/mission/landmark_server/include/landmark_server/landmark_server.hpp b/mission/landmark_server/include/landmark_server/landmark_server.hpp index 7c3115a..9ce8685 100644 --- a/mission/landmark_server/include/landmark_server/landmark_server.hpp +++ b/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 - get_convex_hull(const shape_msgs::msg::SolidPrimitive &solid_primitive); + get_convex_hull(const vortex_msgs::msg::Landmark& landmark); void get_grid(); diff --git a/mission/landmark_server/params/landmark_server_params.yaml b/mission/landmark_server/params/landmark_server_params.yaml index 8455dd8..c334181 100644 --- a/mission/landmark_server/params/landmark_server_params.yaml +++ b/mission/landmark_server/params/landmark_server_params.yaml @@ -1,4 +1,5 @@ landmark_server_node: ros__parameters: fixed_frame: world - target_frame: base_link \ No newline at end of file + target_frame: base_link + wall_width: 0.1 \ No newline at end of file diff --git a/mission/landmark_server/src/grid_manager.cpp b/mission/landmark_server/src/grid_manager.cpp index 492c129..b727165 100644 --- a/mission/landmark_server/src/grid_manager.cpp +++ b/mission/landmark_server/src/grid_manager.cpp @@ -3,113 +3,22 @@ namespace landmark_server { -// GridManager::GridManager(std::vector 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> -// (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 &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 &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((xc - r) / resolution_ + width_ / 2); - int ymin = static_cast((yc - r) / resolution_ + width_ / 2); - int xmax = static_cast((xc + r) / resolution_ + width_ / 2); - int ymax = static_cast((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 &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(min_x / resolution_ + width_ / 2); - // int ymin = static_cast(min_y / resolution_ + height_ / 2); - // int xmax = static_cast(max_x / resolution_ + width_ / 2); - // int ymax = static_cast(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(vertices(0, i) / resolution_ + width_ / 2); int y0 = static_cast(vertices(1, i) / resolution_ + height_ / 2); int x1 = static_cast(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 &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); diff --git a/mission/landmark_server/src/landmark_server.cpp b/mission/landmark_server/src/landmark_server.cpp index 1cc35ed..8fc4f17 100644 --- a/mission/landmark_server/src/landmark_server.cpp +++ b/mission/landmark_server/src/landmark_server.cpp @@ -47,6 +47,8 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options) declare_parameter("fixed_frame", "world"); declare_parameter("target_frame", "base_link"); + declare_parameter("wall_width", 0.1); + // Create the act. action_server_ = rclcpp_action::create_server( @@ -61,17 +63,49 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options) } Eigen::Array LandmarkServerNode::get_convex_hull( - const shape_msgs::msg::SolidPrimitive &solid_primitive) { + const vortex_msgs::msg::Landmark &landmark) { + if (landmark.landmark_type == vortex_msgs::msg::Landmark::WALL) { + Eigen::Array hull(2, 4); + float x1 = landmark.shape.polygon.points[0].x; + float y1 = landmark.shape.polygon.points[0].y; + float x2 = landmark.shape.polygon.points[1].x; + float y2 = landmark.shape.polygon.points[1].y; + + // Calculate the direction vector + float dx = x2 - x1; + float dy = y2 - y1; + + // Normalize the direction vector + float length = std::sqrt(dx * dx + dy * dy); + dx /= length; + dy /= length; + + // Perpendicular vector with the given width + float wall_width = get_parameter("wall_width").as_double(); + float wx = -dy * wall_width/2; // half the width to each side + float wy = dx * wall_width/2; + + // Calculate the four points of the rectangle in clockwise order + hull(0, 0) = x1 + wx; + hull(1, 0) = y1 + wy; + hull(0, 1) = x2 + wx; + hull(1, 1) = y2 + wy; + hull(0, 2) = x2 - wx; + hull(1, 2) = y2 - wy; + hull(0, 3) = x1 - wx; + hull(1, 3) = y1 - wy; + return hull; + } pcl::PointCloud cluster; - cluster.resize(solid_primitive.polygon.points.size()); - for (size_t i = 0; i < solid_primitive.polygon.points.size(); i++) { - cluster.points[i].x = solid_primitive.polygon.points[i].x; - cluster.points[i].y = solid_primitive.polygon.points[i].y; + cluster.resize(landmark.shape.polygon.points.size()); + for (size_t i = 0; i < landmark.shape.polygon.points.size(); i++) { + cluster.points[i].x = landmark.shape.polygon.points[i].x; + cluster.points[i].y = landmark.shape.polygon.points[i].y; cluster.points[i].z = 1.0; } sensor_msgs::msg::PointCloud2 cluster_msg; pcl::toROSMsg(cluster, cluster_msg); - cluster_msg.header.frame_id = "world"; + cluster_msg.header.frame_id = get_parameter("fixed_frame").as_string(); cluster_publisher_->publish(cluster_msg); pcl::PointCloud convex_hull; pcl::ConvexHull chull; @@ -80,16 +114,13 @@ Eigen::Array LandmarkServerNode::get_convex_hull( chull.reconstruct(convex_hull); sensor_msgs::msg::PointCloud2 hull_msg; pcl::toROSMsg(convex_hull, hull_msg); - hull_msg.header.frame_id = "world"; + hull_msg.header.frame_id = get_parameter("fixed_frame").as_string(); convex_hull_publisher_->publish(hull_msg); - RCLCPP_INFO(this->get_logger(), "Convex hull size: %ld", convex_hull.size()); Eigen::Array hull(2, convex_hull.size()); for (size_t i = 0; i < convex_hull.size(); i++) { hull(0, i) = convex_hull.points[i].x; hull(1, i) = convex_hull.points[i].y; } - RCLCPP_INFO(this->get_logger(), "Convex hull: %ld", hull.cols()); - std::cout << "HUll" << hull << std::endl; return hull; } @@ -165,12 +196,11 @@ void LandmarkServerNode::landmarksRecievedCallback( } else { // Add the new landmark grid_manager_->update_grid(grid_msg_.data.data(), - get_convex_hull(landmark.shape), 200); + get_convex_hull(landmark), 200); storedLandmarks_->landmarks.push_back(landmark); } continue; } - if (it == storedLandmarks_->landmarks.end()) { RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), 5000, @@ -179,26 +209,16 @@ void LandmarkServerNode::landmarksRecievedCallback( } grid_manager_->update_grid(grid_msg_.data.data(), - get_convex_hull((*it).shape), -200); + get_convex_hull((*it)), -200); if (landmark.action == vortex_msgs::msg::Landmark::REMOVE_ACTION) { storedLandmarks_->landmarks.erase(it); } else if (landmark.action == vortex_msgs::msg::Landmark::UPDATE_ACTION) { grid_manager_->update_grid(grid_msg_.data.data(), - get_convex_hull(landmark.shape), 200); + get_convex_hull(landmark), 200); *it = landmark; } } - std_msgs::msg::Header header; - - // header.frame_id = get_parameter("fixed_frame").as_string(); - // header.stamp = rclcpp::Clock().now(); - - // // Convert the Eigen array to std::vector - // auto grid_eigen = grid_manager_->get_grid(); - // std::vector grid_data(grid_eigen.data(), grid_eigen.data() + - // grid_eigen.size()); grid_msg_.header = header; grid_msg_.data = - // grid_data; grid_msg_.header.stamp = rclcpp::Clock().now(); // Publish the landmarks diff --git a/mission/map_manager/src/map_manager_ros.cpp b/mission/map_manager/src/map_manager_ros.cpp index b2b9987..be5eba8 100644 --- a/mission/map_manager/src/map_manager_ros.cpp +++ b/mission/map_manager/src/map_manager_ros.cpp @@ -17,7 +17,7 @@ MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) qos_profile_transient_local); map_origin_sub_ = this->create_subscription( - "map_origin", qos_transient_local, + "/map/origin", qos_transient_local, std::bind(&MapManagerNode::mapOriginCallback, this, std::placeholders::_1)); @@ -28,7 +28,7 @@ MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) declare_parameter("map_resolution", 0.2); declare_parameter("map_width", 1000); declare_parameter("map_height", 1000); - declare_parameter("map_frame_id", "world"); + declare_parameter("frame_id", "map"); landmask_file_ = get_parameter("landmask_file").as_string(); landmask_pub_ = this->create_publisher( @@ -187,7 +187,7 @@ MapManagerNode::readPolygonFromFile(const std::string &filename) { geometry_msgs::msg::PolygonStamped MapManagerNode::processCoordinates( const std::vector> &coordinates) { geometry_msgs::msg::PolygonStamped polygon; - polygon.header.frame_id = get_parameter("map_frame_id").as_string(); + polygon.header.frame_id = get_parameter("frame_id").as_string(); polygon.header.stamp = this->now(); std::cout << "coords size: " << coordinates.size() << "\n"; for (const auto &coord : coordinates) { @@ -204,7 +204,7 @@ geometry_msgs::msg::PolygonStamped MapManagerNode::processCoordinates( nav_msgs::msg::OccupancyGrid MapManagerNode::createOccupancyGrid() { nav_msgs::msg::OccupancyGrid grid; - grid.header.frame_id = get_parameter("map_frame_id").as_string(); + grid.header.frame_id = get_parameter("frame_id").as_string(); grid.header.stamp = this->now(); grid.info.resolution = get_parameter("map_resolution").as_double(); grid.info.width = get_parameter("map_width").as_int(); @@ -319,8 +319,8 @@ void MapManagerNode::fillOutsidePolygon( int outside_value = 50; // Set occupancy value for outside (0-100) // Iterate over each pixel in the grid - for (int x = 0; x < grid.info.width; x++) { - for (int y = 0; y < grid.info.height; y++) { + for (size_t x = 0; x < grid.info.width; x++) { + for (size_t y = 0; y < grid.info.height; y++) { if (!isPointInsidePolygon(x, y, polygon, grid)) { grid.data[y * grid.info.width + x] = outside_value; }