Merge branch 'test/29.04' of https://github.com/vortexntnu/vortex-asv …

…into test/29.04

asv_setup/config/sitaw/landmark_server_params.yaml

@@ -2,4 +2,5 @@ landmark_server_node:
   ros__parameters:
     fixed_frame: map
     target_frame: base_link
-    wall_width: 0.1
+    wall_width: 0.1
+    use_grid: false

asv_setup/config/sitaw/seapath_params.yaml

@@ -1,7 +1,4 @@
 seapath_ros_driver_node:
   ros__parameters:
     UDP_IP: "10.0.1.10"  # Source IP of the host sender
-    UDP_PORT: 31421  # Port at which to receive UDP semgents
-    use_predef_map_origin: false
-    map_origin_lat: 0.0
-    map_origin_lon: 0.0
+    UDP_PORT: 31421  # Port at which to receive UDP segment

mission/landmark_server/include/landmark_server/grid_manager.hpp

@@ -16,20 +16,31 @@ class GridManager {
 
   ~GridManager() = default;
 
+  /**
+   * @brief Update the grid with a polygon and a value to update corresponding
+   * cells with.
+   */
   void update_grid(int8_t *grid,
-                   const Eigen::Array<float, 2, Eigen::Dynamic> &polygon,
+                   const Eigen::Array<float, 2, Eigen::Dynamic> &vertices,
                    int value);
 
-  void handle_polygon(int8_t *grid,
-                      const Eigen::Array<float, 2, Eigen::Dynamic> &vertices,
-                      int value);
-
+  /**
+   * @brief Draw a line on the grid and update the value of the cells it passes
+   * through.
+   */
   void draw_line(int x0, int y0, int x1, int y1, int8_t *grid, int value);
 
+  /**
+   * @brief Fill a polygon on the grid and update the value of the cells it
+   * covers.
+   */
   void fill_polygon(int8_t *grid,
                     const Eigen::Array<float, 2, Eigen::Dynamic> &polygon,
                     int value);
 
+  /**
+   * @brief Check if a point is inside a polygon.
+   */
   bool point_in_polygon(int x, int y,
                         const Eigen::Array<float, 2, Eigen::Dynamic> &polygon);
 

mission/landmark_server/params/landmark_server_params.yaml

@@ -2,4 +2,5 @@ landmark_server_node:
   ros__parameters:
     fixed_frame: world
     target_frame: base_link
-    wall_width: 0.1
+    wall_width: 0.1
+    use_grid: false

mission/landmark_server/src/grid_manager.cpp

@@ -7,12 +7,6 @@ GridManager::GridManager(float resolution, uint32_t height, uint32_t width)
     : resolution_(resolution), height_(height), width_(width) {}
 
 void GridManager::update_grid(
-    int8_t *grid, const Eigen::Array<float, 2, Eigen::Dynamic> &polygon,
-    int value) {
-  handle_polygon(grid, polygon, value);
-}
-
-void GridManager::handle_polygon(
     int8_t *grid, const Eigen::Array<float, 2, Eigen::Dynamic> &vertices,
     int value) {
   // Convert to grid coordinates
@@ -41,10 +35,8 @@ void GridManager::fill_polygon(
   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;
-        }
+          y < static_cast<int>(height_) && point_in_polygon(x, y, polygon)) {
+        grid[y * width_ + x] += value;
       }
     }
   }

mission/landmark_server/src/landmark_server.cpp

@@ -59,6 +59,7 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options)
   declare_parameter<std::string>("fixed_frame", "world");
   declare_parameter<std::string>("target_frame", "base_link");
   declare_parameter<double>("wall_width", 0.1);
+  declare_parameter<bool>("use_grid", "false");
 
   // Create the act.
   action_server_ = rclcpp_action::create_server<Action>(
@@ -113,19 +114,13 @@ Eigen::Array<float, 2, Eigen::Dynamic> LandmarkServerNode::get_convex_hull(
     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 = get_parameter("fixed_frame").as_string();
-  cluster_publisher_->publish(cluster_msg);
+  // Compute the convex hull of the cluster
   pcl::PointCloud<pcl::PointXYZ> convex_hull;
   pcl::ConvexHull<pcl::PointXYZ> chull;
   chull.setDimension(2);
   chull.setInputCloud(cluster.makeShared());
   chull.reconstruct(convex_hull);
-  sensor_msgs::msg::PointCloud2 hull_msg;
-  pcl::toROSMsg(convex_hull, hull_msg);
-  hull_msg.header.frame_id = get_parameter("fixed_frame").as_string();
-  convex_hull_publisher_->publish(hull_msg);
+
   Eigen::Array<float, 2, Eigen::Dynamic> hull(2, convex_hull.size());
   for (size_t i = 0; i < convex_hull.size(); i++) {
     hull(0, i) = convex_hull.points[i].x;
@@ -184,7 +179,7 @@ void LandmarkServerNode::landmarksRecievedCallback(
     return;
   }
 
-  if (grid_manager_ == nullptr) {
+  if (this->get_parameter("use_grid").as_bool() && grid_manager_ == nullptr) {
     RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
                                 "Grid manager not initialized");
     return;
@@ -205,8 +200,11 @@ void LandmarkServerNode::landmarksRecievedCallback(
             "Requested to add already existing landmark");
       } else {
         // Add the new landmark
-        grid_manager_->update_grid(grid_msg_.data.data(),
-                                   get_convex_hull(landmark), 200);
+
+        if (this->get_parameter("use_grid").as_bool()) {
+          grid_manager_->update_grid(grid_msg_.data.data(),
+                                     get_convex_hull(landmark), 200);
+        }
         storedLandmarks_->landmarks.push_back(landmark);
       }
       continue;
@@ -218,14 +216,18 @@ void LandmarkServerNode::landmarksRecievedCallback(
       continue;
     }
 
-    grid_manager_->update_grid(grid_msg_.data.data(), get_convex_hull((*it)),
-                               -200);
+    if (this->get_parameter("use_grid").as_bool()) {
+      grid_manager_->update_grid(grid_msg_.data.data(),
+                                 get_convex_hull(landmark), -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), 200);
+      if (this->get_parameter("use_grid").as_bool()) {
+        grid_manager_->update_grid(grid_msg_.data.data(),
+                                   get_convex_hull(landmark), 200);
+      }
       *it = landmark;
     }
   }
@@ -354,9 +356,10 @@ vortex_msgs::msg::OdometryArray LandmarkServerNode::filterLandmarks(
 
   for (const auto &landmark : storedLandmarks_->landmarks) {
 
-    if (filter_type == 6 && distance == 0.0) {
+    // filter_type 0 returns all landmarks
+    if (filter_type == 0 && distance == 0.0) {
       filteredLandmarksOdoms.odoms.push_back(landmark.odom);
-    } else if (filter_type == 6) {
+    } else if (filter_type == 0) {
 
       if (calculateDistance(landmark.odom.pose.pose.position,
                             landmark.odom.header) <= distance) {

mission/map_manager/include/map_manager/map_manager_ros.hpp

@@ -1,11 +1,13 @@
 #ifndef MAP_MANAGER_HPP
 #define MAP_MANAGER_HPP
 
+#include "geometry_msgs/msg/transform_stamped.hpp"
 #include "nav_msgs/srv/get_map.hpp"
 #include <geometry_msgs/msg/polygon_stamped.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/nav_sat_fix.hpp>
+#include <tf2_ros/static_transform_broadcaster.h>
 
 #include <pcl/PointIndices.h>
 #include <pcl/point_cloud.h>
@@ -45,6 +47,15 @@ class MapManagerNode : public rclcpp::Node {
   constexpr double deg2rad(double degrees) const;
   std::array<double, 2> lla2flat(double lat, double lon) const;
   std::array<double, 2> flat2lla(double px, double py) const;
+  /**
+   * @brief Publishes static transform from world NED to world SEU to use for
+   * foxglove visualization.
+   *
+   */
+  void publish_foxglove_vis_frame(const rclcpp::Time &time) const;
+
+  void publish_map_to_odom_tf(double map_lat, double map_lon,
+                              const rclcpp::Time &time) const;
 
   geometry_msgs::msg::PolygonStamped
   processCoordinates(const std::vector<std::array<double, 2>> &coordinates);
@@ -62,17 +73,19 @@ class MapManagerNode : public rclcpp::Node {
   void fillOutsidePolygon(nav_msgs::msg::OccupancyGrid &grid,
                           const geometry_msgs::msg::PolygonStamped &polygon);
 
-  rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr map_origin_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr odom_origin_sub_;
   rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
   rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr
       landmask_pub_;
   rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr grid_service_;
+  rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr map_origin_pub_;
   bool map_origin_set_ = false;
   bool use_predef_landmask_;
   double map_origin_lat_;
   double map_origin_lon_;
   std::string landmask_file_;
   std::string grid_frame_id_;
+  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;
 };
 
 } // namespace map_manager

mission/map_manager/params/map_manager_params.yaml

@@ -1,15 +1,10 @@
 map_manager_node:
   ros__parameters:
-    use_predef_landmask: true
-    landmask_file: "src/vortex-asv/mission/map_manager/params/land_polygon.yaml"
+    use_predef_landmask: false
     map_resolution: 0.2
     map_width: 1000
     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: 0.0
+    map_origin_lon: 0.0
+    use_predef_map_origin: false