Merge branch 'main' into q9i/nix-launch

all_seaing_bringup/launch/shoreside.launch.py

@@ -67,7 +67,7 @@ def generate_launch_description():
     return LaunchDescription(
         [
             DeclareLaunchArgument(
-                "launch_rviz", default_value="true", choices=["true", "false"]
+                "launch_rviz", default_value="false", choices=["true", "false"]
             ),
             DeclareLaunchArgument(
                 "use_lora", default_value="false", choices=["true", "false"]

all_seaing_bringup/launch/sim.launch.py

@@ -113,7 +113,10 @@ def launch_setup(context, *args, **kwargs):
         parameters=[
             {
                 "color_label_mappings_file": color_label_mappings,
-            }
+            },
+            {
+                "is_sim": True,
+            },
         ],
     )
 
@@ -153,13 +156,9 @@ def launch_setup(context, *args, **kwargs):
             ("point_cloud", "point_cloud/filtered"),
         ],
         parameters=[
-            {"robot_frame_id": "wamv/wamv/base_link"},
             {"obstacle_size_min": 2},
             {"obstacle_size_max": 60},
             {"clustering_distance": 1.0},
-            {"obstacle_seg_thresh": 10.0},
-            {"obstacle_drop_thresh": 1.0},
-            {"polygon_area_thresh": 100000.0},
         ],
     )
 

all_seaing_bringup/launch/vehicle.launch.py

@@ -118,8 +118,7 @@ def launch_setup(context, *args, **kwargs):
             ("point_cloud", "/velodyne_points"),
         ],
         parameters=[
-            #{"range_x": [-0.5, 1.5]},
-            #{"range_y": [0.0, 3.0]},
+            {"range_radius": [0.5, 100000.0]},
         ],
     )
 
@@ -141,12 +140,9 @@ def launch_setup(context, *args, **kwargs):
             ("point_cloud", "point_cloud/filtered"),
         ],
         parameters=[
-            {"obstacle_size_min": 2},
-            {"obstacle_size_max": 1000},
-            {"clustering_distance": 1.0},
-            {"obstacle_seg_thresh": 10.0},
-            {"obstacle_drop_thresh": 1.0},
-            {"polygon_area_thresh": 100000.0},
+            {"obstacle_size_min": 20},
+            {"obstacle_size_max": 800},
+            {"clustering_distance": 0.1},
         ],
     )
 

all_seaing_perception/all_seaing_perception/obstacle_detector.cpp

@@ -27,18 +27,12 @@ class ObstacleDetector : public rclcpp::Node {
         this->declare_parameter<int>("obstacle_size_min", 20);
         this->declare_parameter<int>("obstacle_size_max", 100000);
         this->declare_parameter<double>("clustering_distance", 0.75);
-        this->declare_parameter<double>("obstacle_seg_thresh", 1.0);
-        this->declare_parameter<double>("obstacle_drop_thresh", 1.0);
-        this->declare_parameter<double>("polygon_area_thresh", 100000.0);
 
         // Initialize member variables from parameters
         m_global_frame_id = this->get_parameter("global_frame_id").as_string();
         m_obstacle_size_min = this->get_parameter("obstacle_size_min").as_int();
         m_obstacle_size_max = this->get_parameter("obstacle_size_max").as_int();
         m_clustering_distance = this->get_parameter("clustering_distance").as_double();
-        m_obstacle_seg_thresh = this->get_parameter("obstacle_seg_thresh").as_double();
-        m_obstacle_drop_thresh = this->get_parameter("obstacle_drop_thresh").as_double();
-        m_polygon_area_thresh = this->get_parameter("polygon_area_thresh").as_double();
 
         // Initialize tf_listener pointer
         m_tf_buffer = std::make_unique<tf2_ros::Buffer>(this->get_clock());
@@ -139,9 +133,6 @@ class ObstacleDetector : public rclcpp::Node {
     int m_obstacle_size_min;
     int m_obstacle_size_max;
     double m_clustering_distance;
-    double m_obstacle_seg_thresh;
-    double m_obstacle_drop_thresh;
-    double m_polygon_area_thresh;
 
     // Transform variables
     std::shared_ptr<tf2_ros::TransformListener> m_tf_listener{nullptr};

all_seaing_perception/all_seaing_perception/point_cloud_filter.cpp

@@ -25,6 +25,7 @@ class PointCloudFilter : public rclcpp::Node {
         this->declare_parameter<std::vector<double>>("range_intensity", {0.0, 100000.0});
         this->declare_parameter<std::vector<double>>("range_x", {-100000.0, 100000.0});
         this->declare_parameter<std::vector<double>>("range_y", {-100000.0, 100000.0});
+        this->declare_parameter<std::vector<double>>("range_z", {-100000.0, 100000.0});
         this->declare_parameter<double>("leaf_size", 0.0);
 
         // Initialize tf_listener pointer
@@ -46,6 +47,7 @@ class PointCloudFilter : public rclcpp::Node {
         m_range_intensity = this->get_parameter("range_intensity").as_double_array();
         m_range_x = this->get_parameter("range_x").as_double_array();
         m_range_y = this->get_parameter("range_y").as_double_array();
+        m_range_z = this->get_parameter("range_z").as_double_array();
         m_leaf_size = this->get_parameter("leaf_size").as_double();
     }
 
@@ -73,7 +75,7 @@ class PointCloudFilter : public rclcpp::Node {
                       pcl::PointCloud<pcl::PointXYZI>::Ptr &out_cloud_ptr) {
         // Keep point if in radius thresholds, intensity threshold, and is finite
         for (const auto &point : in_cloud_ptr->points) {
-            double radius = sqrt(point.x * point.x + point.y * point.y + point.z * point.z);
+            double radius = sqrt(point.x * point.x + point.y * point.y);
 
             // Convert point to map
             geometry_msgs::msg::Point point_msg;
@@ -85,6 +87,7 @@ class PointCloudFilter : public rclcpp::Node {
 
             if (m_range_x[0] <= point_tf.x && point_tf.x <= m_range_x[1] &&
                 m_range_y[0] <= point_tf.y && point_tf.y <= m_range_y[1] &&
+                m_range_z[0] <= point_tf.z && point_tf.z <= m_range_z[1] &&
                 m_range_radius[0] <= radius && radius <= m_range_radius[1] &&
                 m_range_intensity[0] <= point.intensity &&
                 point.intensity <= m_range_intensity[1] && 
@@ -136,6 +139,7 @@ class PointCloudFilter : public rclcpp::Node {
     std::vector<double> m_range_intensity;
     std::vector<double> m_range_x;
     std::vector<double> m_range_y;
+    std::vector<double> m_range_z;
     double m_leaf_size;
 };
 

all_seaing_perception/all_seaing_perception/point_cloud_image_overlay.cpp

@@ -14,7 +14,8 @@ void PclImageOverlay::pc_image_fusion_cb(
     const sensor_msgs::msg::PointCloud2::ConstSharedPtr &in_cloud_msg) {
 
     // Only continue if intrinsics has been initialized
-    if (!m_cam_model.initialized()) return;
+    if (!m_cam_model.initialized())
+        return;
 
     // Get transform the first iteration
     if (!m_pc_cam_tf.has_value())
@@ -31,33 +32,36 @@ void PclImageOverlay::pc_image_fusion_cb(
 
     // Transform in_cloud_msg and convert PointCloud2 to PCL PointCloud
     sensor_msgs::msg::PointCloud2 in_cloud_tf;
-    tf2::doTransform<sensor_msgs::msg::PointCloud2>(*in_cloud_msg, in_cloud_tf, m_pc_cam_tf.value());
+    tf2::doTransform<sensor_msgs::msg::PointCloud2>(*in_cloud_msg, in_cloud_tf,
+                                                    m_pc_cam_tf.value());
     pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_tf_ptr(new pcl::PointCloud<pcl::PointXYZI>);
     pcl::fromROSMsg(in_cloud_tf, *in_cloud_tf_ptr);
 
     for (pcl::PointXYZI &point_tf : in_cloud_tf_ptr->points) {
 
         // Ignore points which are NaN
-        if (isnan(point_tf.x) || isnan(point_tf.y) || isnan(point_tf.z)) continue;
+        if (isnan(point_tf.x) || isnan(point_tf.y) || isnan(point_tf.z))
+            continue;
 
         // Project 3D point onto the image plane using the intrinsic matrix.
         // Gazebo has a different coordinate system, so the y, z, and x coordinates are modified.
-        cv::Point2d xy_rect = m_is_sim ?
-            m_cam_model.project3dToPixel(cv::Point3d(point_tf.y, point_tf.z, -point_tf.x)) :
-            m_cam_model.project3dToPixel(cv::Point3d(point_tf.x, point_tf.y, point_tf.z));
+        cv::Point2d xy_rect =
+            m_is_sim
+                ? m_cam_model.project3dToPixel(cv::Point3d(point_tf.y, point_tf.z, -point_tf.x))
+                : m_cam_model.project3dToPixel(cv::Point3d(point_tf.x, point_tf.y, point_tf.z));
 
         // Plot projected point onto image if within bounds and in front of the boat
-        if ((xy_rect.x >= 0) && (xy_rect.x < m_cam_model.cameraInfo().width) &&
-            (xy_rect.y >= 0) && (xy_rect.y < m_cam_model.cameraInfo().height) &&
+        if ((xy_rect.x >= 0) && (xy_rect.x < m_cam_model.cameraInfo().width) && (xy_rect.y >= 0) &&
+            (xy_rect.y < m_cam_model.cameraInfo().height) &&
             (m_is_sim ? point_tf.x >= 0 : point_tf.z >= 0)) {
             cv::circle(cv_ptr->image, cv::Point(xy_rect.x, xy_rect.y), 2, cv::Scalar(255, 0, 0), 4);
         }
-        
+
         // Debug statements
-        RCLCPP_DEBUG(this->get_logger(), "3D point in camera frame: (%.2f, %.2f, %.2f)",
-                point_tf.x, point_tf.y, point_tf.z);
-        RCLCPP_DEBUG(this->get_logger(), "2D point in pixel coordinates: (%.2f, %.2f)",
-                xy_rect.x, xy_rect.y);
+        RCLCPP_DEBUG(this->get_logger(), "3D point in camera frame: (%.2f, %.2f, %.2f)", point_tf.x,
+                     point_tf.y, point_tf.z);
+        RCLCPP_DEBUG(this->get_logger(), "2D point in pixel coordinates: (%.2f, %.2f)", xy_rect.x,
+                     xy_rect.y);
     }
     m_image_pub->publish(*cv_ptr->toImageMsg());
 }

all_seaing_perception/all_seaing_perception/yolov8_node.py

@@ -79,7 +79,6 @@ def __init__(self) -> None:
             self.model = YOLO(pt_path)
         else:
             self.get_logger().error(f"Both model paths do not exist :( TensorRT: {engine_path} and pt: {pt_path}")
-
         # Setup QoS profile
         image_qos_profile = QoSProfile(
             reliability=QoSReliabilityPolicy.BEST_EFFORT,