diff --git a/build_and_launch.Unix.sh b/build_and_launch.Unix.sh new file mode 100644 index 0000000..08d8af6 --- /dev/null +++ b/build_and_launch.Unix.sh @@ -0,0 +1,93 @@ +#!/usr/bin/env bash +set -euo pipefail + +# ——— CONFIGURE ——— +# Adjust these to match your ROS 2 distro and workspace layout: +ROS_DISTRO=humble +ROS_INSTALL=/opt/ros/$ROS_DISTRO/setup.bash + +# Map menu keys to " " +declare -A options=( + ["1"]="vision_cone_detector vision_pipeline.launch.py" + ["2"]="lidar_cone_detector lidar_pipeline.launch.py" + ["3"]="mfe_eufs_sim mfe_eufs_sim.launch.py" + # add or remove entries as needed... +) +# ————————————————— + +# 1) Source base ROS 2 +if [ -f "$ROS_INSTALL" ]; then + echo -e "\n=== Sourcing ROS 2 base ($ROS_DISTRO) ===" + set +u + source "$ROS_INSTALL" + set -u +else + echo "ERROR: cannot find ROS install at $ROS_INSTALL" >&2 + exit 1 +fi + +# 2) Ask user which packages to skip +echo -e "\n=== Skip Packages ===" +read -rp "Enter packages to skip (space-separated, or leave empty for none): " skip_input +skip_args=() +if [[ -n "$skip_input" ]]; then + read -ra skip_args <<< "$skip_input" +fi + +# 3) Build or Skip +echo -e "\n=== Build Options ===" +echo "1) Build workspace" +echo "2) Skip build (use existing install)" +read -rp "Enter choice [1/2]: " build_choice + +WS_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +cd "$WS_ROOT" + +if [[ "$build_choice" == "1" ]]; then + echo "Building workspace..." + if [ ${#skip_args[@]} -gt 0 ]; then + echo "Skipping packages: ${skip_args[*]}" + colcon build --symlink-install --packages-ignore "${skip_args[@]}" + else + colcon build --symlink-install + fi +elif [[ "$build_choice" == "2" ]]; then + echo "Skipping build step." +else + echo "Invalid build choice: '$build_choice'" >&2 + exit 1 +fi + +# 4) Source your workspace overlay +echo -e "\n=== Sourcing workspace overlay ===" +set +u +source "$WS_ROOT/install/setup.bash" +set -u + +# 5) Ask if sample data should be streamed +echo -e "\n=== Stream Sample Data ===" +read -rp "Stream sample data? [Y/N]: " stream_choice +if [[ "$stream_choice" =~ ^[Yy]$ ]]; then + load_file_arg="load_file:=True" +else + load_file_arg="load_file:=False" +fi + +# 6) Show launch menu +echo -e "\n=== Select a launch file to run ===" +for key in "${!options[@]}"; do + pkg_and_file=(${options[$key]}) + printf "%2s) %s/%s\n" "$key" "${pkg_and_file[0]}" "${pkg_and_file[1]}" +done + +read -rp "Enter launch choice: " choice + +# 7) Launch +if [[ -n "${options[$choice]:-}" ]]; then + pkg_and_file=(${options[$choice]}) + echo -e "\nLaunching ➜ ros2 launch ${pkg_and_file[0]} ${pkg_and_file[1]} ${load_file_arg}\n" + ros2 launch "${pkg_and_file[0]}" "${pkg_and_file[1]}" "${load_file_arg}" +else + echo "Invalid choice: '$choice'" >&2 + exit 1 +fi diff --git a/ros2/src/mfe_perception/lidar_cone_detector/CMakeLists.txt b/ros2/src/mfe_perception/lidar_cone_detector/CMakeLists.txt index 720c23e..c089f3b 100644 --- a/ros2/src/mfe_perception/lidar_cone_detector/CMakeLists.txt +++ b/ros2/src/mfe_perception/lidar_cone_detector/CMakeLists.txt @@ -1,82 +1,81 @@ cmake_minimum_required(VERSION 3.8) project(lidar_cone_detector) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies -find_package(ament_cmake_auto REQUIRED) -find_package(ament_cmake_python REQUIRED) -find_package(rclpy REQUIRED) -# eigen3 has dependencies that don't work with ament_cmake, use side package to import connection -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3) -find_package(pointcloud_to_laserscan REQUIRED) -ament_auto_find_build_dependencies() +# Core ament +find_package(ament_cmake REQUIRED) -ament_auto_generate_code() +# ROS 2 deps +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +# If you publish Cone/Track: +find_package(mfe_msgs REQUIRED) -ament_auto_add_library(${PROJECT_NAME} - DIRECTORY src -) +# Eigen (via helper module if you need it) +find_package(eigen3_cmake_module QUIET) +find_package(Eigen3 REQUIRED) -ament_auto_add_executable(lidar_preprocessor - src/lidar_preprocessor.cpp -) +# PCL + conversions +find_package(pcl_conversions REQUIRED) -ament_auto_add_executable(ground_plane_removal - src/ground_plane_removal.cpp -) +find_package(PCL REQUIRED COMPONENTS common io filters segmentation visualization) -ament_auto_add_executable(file_loader - src/file_loader.cpp -) +# Optional other deps +# find_package(pointcloud_to_laserscan QUIET) -target_link_libraries(ground_plane_removal ${PCL_LIBRARIES}) +include_directories(${PCL_INCLUDE_DIRS}) +add_definitions(${PCL_DEFINITIONS}) -ament_python_install_package(${PROJECT_NAME}) +# ---- Executables ---- +add_executable(cone_detector_node + src/cone_detector.cpp + src/dbscan.cpp +) +ament_target_dependencies(cone_detector_node + rclcpp sensor_msgs geometry_msgs std_msgs pcl_conversions mfe_msgs +) +target_link_libraries(cone_detector_node ${PCL_LIBRARIES}) -install(PROGRAMS - scripts/cone_detector_node.py - DESTINATION lib/${PROJECT_NAME} - RENAME cone_detector_node +add_executable(lidar_preprocessor src/lidar_preprocessor.cpp) +ament_target_dependencies(lidar_preprocessor + rclcpp sensor_msgs pcl_conversions ) +target_link_libraries(lidar_preprocessor ${PCL_LIBRARIES}) -install(DIRECTORY - launch - dataset - DESTINATION share/${PROJECT_NAME} +add_executable(ground_plane_removal src/ground_plane_removal.cpp) +ament_target_dependencies(ground_plane_removal + rclcpp sensor_msgs pcl_conversions ) +target_link_libraries(ground_plane_removal ${PCL_LIBRARIES}) + +add_executable(file_loader src/file_loader.cpp) +ament_target_dependencies(file_loader + rclcpp sensor_msgs pcl_conversions +) +target_link_libraries(file_loader ${PCL_LIBRARIES}) +# ---- Install ---- install(TARGETS + cone_detector_node lidar_preprocessor ground_plane_removal file_loader - DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION lib/${PROJECT_NAME} ) +install(DIRECTORY launch dataset DESTINATION share/${PROJECT_NAME}) + +# ---- Tests (optional; keep minimal) ---- if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() endif() -ament_export_dependencies(eigen3_cmake_module) -ament_export_dependencies(Eigen3) - -ament_auto_package() - -# tests -if(AMENT_ENABLE_TESTING) - find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(foo_gtest test/my_test.cpp) - target_link_libraries(foo_gtest ${rclcpp_LIBRARIES} ${rmw_connext_cpp_LIBRARIES} ${std_interfaces}) -endif() \ No newline at end of file +ament_package() diff --git a/ros2/src/mfe_perception/lidar_cone_detector/include/lidar_cone_detector/cone_detector.hpp b/ros2/src/mfe_perception/lidar_cone_detector/include/lidar_cone_detector/cone_detector.hpp new file mode 100644 index 0000000..d117ed6 --- /dev/null +++ b/ros2/src/mfe_perception/lidar_cone_detector/include/lidar_cone_detector/cone_detector.hpp @@ -0,0 +1,52 @@ +// include/lidar_cone_detector/cone_detector.hpp +#pragma once + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/qos.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" + +#include "mfe_msgs/msg/cone.hpp" +#include "mfe_msgs/msg/track.hpp" + +// Forward-declare PCL types to keep the header light if you like +namespace pcl { + template class PointCloud; + struct PointXYZ; +} + +namespace lidar_cone_detector { + +class ConeDetectorNode : public rclcpp::Node { +public: + explicit ConeDetectorNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + +private: + // --- Callbacks --- + void onCloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + + // Fill with cluster indices (each cluster is vector of point indices) + void clusterPoints_DBSCAN(const pcl::PointCloud& cloud, + std::vector>& clusters) const; + + void publishDebugClouds(const pcl::PointCloud& all_cluster_pts, + const pcl::PointCloud& centres, + const std_msgs::msg::Header& header); + + // --- ROS I/O --- + rclcpp::Subscription::SharedPtr sub_cloud_; + rclcpp::Publisher::SharedPtr pub_clusters_; + rclcpp::Publisher::SharedPtr pub_centres_; + rclcpp::Publisher::SharedPtr pub_cone_; + rclcpp::Publisher::SharedPtr pub_track_; + + // --- Parameters --- + double eps_{0.5}; // clustering radius (meters) + int min_pts_{3}; // minimum points per cluster +}; + +} // namespace lidar_cone_detector + diff --git a/ros2/src/mfe_perception/lidar_cone_detector/include/lidar_cone_detector/dbscan.h b/ros2/src/mfe_perception/lidar_cone_detector/include/lidar_cone_detector/dbscan.h new file mode 100644 index 0000000..958dfc9 --- /dev/null +++ b/ros2/src/mfe_perception/lidar_cone_detector/include/lidar_cone_detector/dbscan.h @@ -0,0 +1,50 @@ +#ifndef DBSCAN_H +#define DBSCAN_H + +#include +#include + +#define UNCLASSIFIED -1 +#define CORE_POINT 1 +#define BORDER_POINT 2 +#define NOISE -2 +#define SUCCESS 0 +#define FAILURE -3 + +using namespace std; + +typedef struct Point_ +{ + float x, y, z; // X, Y, Z position + int clusterID; // clustered ID +}Point; + +class DBSCAN { +public: + DBSCAN(unsigned int minPts, float eps, vector points){ + m_minPoints = minPts; + m_epsilon = eps; + m_points = points; + m_pointSize = points.size(); + } + ~DBSCAN(){} + + int run(); + vector calculateCluster(Point point); + int expandCluster(Point point, int clusterID); + inline double calculateDistance(const Point& pointCore, const Point& pointTarget); + + int getTotalPointSize() {return m_pointSize;} + int getMinimumClusterSize() {return m_minPoints;} + int getEpsilonSize() {return m_epsilon;} + +public: + vector m_points; + +private: + unsigned int m_pointSize; + unsigned int m_minPoints; + float m_epsilon; +}; + +#endif // DBSCAN_H \ No newline at end of file diff --git a/ros2/src/mfe_perception/lidar_cone_detector/lidar_cone_detector/cone_detector.py b/ros2/src/mfe_perception/lidar_cone_detector/lidar_cone_detector/cone_detector.py index b39bfa0..dd9b1a7 100755 --- a/ros2/src/mfe_perception/lidar_cone_detector/lidar_cone_detector/cone_detector.py +++ b/ros2/src/mfe_perception/lidar_cone_detector/lidar_cone_detector/cone_detector.py @@ -23,10 +23,11 @@ def __init__(self): self.get_logger().info("Cone detector node initialized and spinning...") + # set up quality of service profile qos_profile = QoSProfile( - depth=10, - reliability=ReliabilityPolicy.RELIABLE, - durability=DurabilityPolicy.VOLATILE + depth=10, # keep last 10 messages + reliability=ReliabilityPolicy.RELIABLE, # ensure all messages are received, retry if lost + durability=DurabilityPolicy.VOLATILE # do not store and redistribute messages for late-joining subscribers ) # subscribe to the outputs of the ground removal script diff --git a/ros2/src/mfe_perception/lidar_cone_detector/package.xml b/ros2/src/mfe_perception/lidar_cone_detector/package.xml index eee0a73..69a3b22 100644 --- a/ros2/src/mfe_perception/lidar_cone_detector/package.xml +++ b/ros2/src/mfe_perception/lidar_cone_detector/package.xml @@ -14,19 +14,21 @@ sensor_msgs vision_msgs pcl_msgs + mfe_msgs rclcpp eigen + eigen3 tf2 tf2_eigen tf2_ros pcl_ros pcl_conversions pointcloud_to_laserscan - + geometry_msgs rclpy python3-sklearn - + libpcl-common libpcl-features libpcl-filters diff --git a/ros2/src/mfe_perception/lidar_cone_detector/src/cone_detector.cpp b/ros2/src/mfe_perception/lidar_cone_detector/src/cone_detector.cpp new file mode 100644 index 0000000..d78fd0b --- /dev/null +++ b/ros2/src/mfe_perception/lidar_cone_detector/src/cone_detector.cpp @@ -0,0 +1,141 @@ +#include "../include/lidar_cone_detector/cone_detector.hpp" + +#include +#include +#include +#include +#include +#include "../include/lidar_cone_detector/dbscan.h" + +#include + +using sensor_msgs::msg::PointCloud2; + +namespace lidar_cone_detector { + +ConeDetectorNode::ConeDetectorNode(const rclcpp::NodeOptions& options) +: rclcpp::Node("cone_detector_node", options) { + // Declare/get params + this->declare_parameter("dbscan_epsilon", 0.5); + this->declare_parameter("dbscan_cluster_min_samples", 8); + this->get_parameter("dbscan_epsilon", eps_); + this->get_parameter("dbscan_cluster_min_samples", min_pts_); + + auto sub_qos_cloud = rclcpp::SensorDataQoS(); // best-effort, small depth + auto pub_qos_cloud = rclcpp::QoS(rclcpp::KeepLast(10)); + sub_cloud_ = this->create_subscription( + "pcl/objects", sub_qos_cloud, + std::bind(&ConeDetectorNode::onCloud, this, std::placeholders::_1)); + + pub_clusters_ = this->create_publisher("pcl/objects2", pub_qos_cloud); // clustered points of cones + pub_centres_ = this->create_publisher("pcl/cone_centres", pub_qos_cloud); + + // Keep cones/track as RELIABLE if another node consumes them; for RViz-only, SensorDataQoS is fine too. + + pub_cone_ = this->create_publisher("pcl/cones", pub_qos_cloud); + pub_track_ = this->create_publisher("pcl/track", pub_qos_cloud); + RCLCPP_INFO(this->get_logger(), "cone_detector_node up"); +} + +void ConeDetectorNode::onCloud(const PointCloud2::SharedPtr msg) { + // convert ROS -> PCL and drop NaNs + pcl::PointCloud cloud; + pcl::fromROSMsg(*msg, cloud); + std::vector idx; pcl::removeNaNFromPointCloud(cloud, cloud, idx); + if (cloud.empty()) return; + + //debug + RCLCPP_INFO(this->get_logger(), "Received cloud with %zu points", cloud.size()); + + // cluster + std::vector> clusters; + clusterPoints_DBSCAN(cloud, clusters); + + // build outputs + pcl::PointCloud centres, all_pts; + mfe_msgs::msg::Track track; + for (const auto& ci : clusters) { + if (ci.empty()) continue; + Eigen::Vector3d sum(0,0,0); + for (int k : ci) { + const auto& p = cloud.points[k]; + all_pts.push_back(p); + Eigen::Vector3d temp(p.x,p.y,p.z); + sum += temp; + } + Eigen::Vector3d c = sum / double(ci.size()); + centres.push_back(pcl::PointXYZ(c.x(), c.y(), c.z())); + + mfe_msgs::msg::Cone cone; cone.location.x=c.x(); cone.location.y=c.y(); cone.location.z=c.z(); cone.color=0; + pub_cone_->publish(cone); + track.track.push_back(cone); + } + pub_track_->publish(track); + + publishDebugClouds(all_pts, centres, msg->header); + +} + +void ConeDetectorNode::clusterPoints_DBSCAN( + const pcl::PointCloud& cloud, + std::vector>& clusters) const +{ + clusters.clear(); + if (cloud.empty()) return; + + // 1) Build the library’s Point array, mark all UNCLASSIFIED + std::vector pts; pts.reserve(cloud.size()); + for (const auto& p : cloud.points) { + Point q; + q.x = p.x; q.y = p.y; q.z = p.z; + q.clusterID = UNCLASSIFIED; + pts.push_back(q); + } + + // 2) eps in this lib is COMPARED TO SQUARED DISTANCE → pass eps^2 + const float eps_sq = static_cast(eps_ * eps_); + + DBSCAN db(min_pts_, eps_sq, pts); + db.run(); + + // 3) Group indices by clusterID (>0 are valid clusters) + const auto& labeled = db.m_points; // contains updated clusterID + // find max cluster id + int max_id = 0; + for (const auto& p : labeled) if (p.clusterID > max_id) max_id = p.clusterID; + if (max_id <= 0) return; // only noise/unclassified + + clusters.assign(static_cast(max_id), {}); // 1..max_id + for (size_t i = 0; i < labeled.size(); ++i) { + int id = labeled[i].clusterID; + if (id > 0) clusters[static_cast(id - 1)].push_back(static_cast(i)); + } +} + +void ConeDetectorNode::publishDebugClouds( + const pcl::PointCloud& all_cluster_pts, + const pcl::PointCloud& centres, + const std_msgs::msg::Header& header) { + if (!all_cluster_pts.empty()) { + PointCloud2 out; pcl::toROSMsg(all_cluster_pts, out); out.header = header; + pub_clusters_->publish(out); + } + if (!centres.empty()) { + PointCloud2 out; + pcl::toROSMsg(centres, out); + out.header = header; + pub_centres_->publish(out); + } + // // Debug output + // RCLCPP_INFO(this->get_logger(), "Published %zu cluster points and %zu centres", + // all_cluster_pts.size(), centres.size()); +} + +} // namespace lidar_cone_detector + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/ros2/src/mfe_perception/lidar_cone_detector/src/dbscan.cpp b/ros2/src/mfe_perception/lidar_cone_detector/src/dbscan.cpp new file mode 100644 index 0000000..d77e110 --- /dev/null +++ b/ros2/src/mfe_perception/lidar_cone_detector/src/dbscan.cpp @@ -0,0 +1,91 @@ +#include "../include/lidar_cone_detector/dbscan.h" + +int DBSCAN::run() +{ + int clusterID = 1; + vector::iterator iter; + for(iter = m_points.begin(); iter != m_points.end(); ++iter) + { + if ( iter->clusterID == UNCLASSIFIED ) + { + if ( expandCluster(*iter, clusterID) != FAILURE ) + { + clusterID += 1; + } + } + } + + return 0; +} + +int DBSCAN::expandCluster(Point point, int clusterID) +{ + vector clusterSeeds = calculateCluster(point); + + if ( clusterSeeds.size() < m_minPoints ) + { + point.clusterID = NOISE; + return FAILURE; + } + else + { + int index = 0, indexCorePoint = 0; + vector::iterator iterSeeds; + for( iterSeeds = clusterSeeds.begin(); iterSeeds != clusterSeeds.end(); ++iterSeeds) + { + m_points.at(*iterSeeds).clusterID = clusterID; + if (m_points.at(*iterSeeds).x == point.x && m_points.at(*iterSeeds).y == point.y && m_points.at(*iterSeeds).z == point.z ) + { + indexCorePoint = index; + } + ++index; + } + clusterSeeds.erase(clusterSeeds.begin()+indexCorePoint); + + for( vector::size_type i = 0, n = clusterSeeds.size(); i < n; ++i ) + { + vector clusterNeighors = calculateCluster(m_points.at(clusterSeeds[i])); + + if ( clusterNeighors.size() >= m_minPoints ) + { + vector::iterator iterNeighors; + for ( iterNeighors = clusterNeighors.begin(); iterNeighors != clusterNeighors.end(); ++iterNeighors ) + { + if ( m_points.at(*iterNeighors).clusterID == UNCLASSIFIED || m_points.at(*iterNeighors).clusterID == NOISE ) + { + if ( m_points.at(*iterNeighors).clusterID == UNCLASSIFIED ) + { + clusterSeeds.push_back(*iterNeighors); + n = clusterSeeds.size(); + } + m_points.at(*iterNeighors).clusterID = clusterID; + } + } + } + } + + return SUCCESS; + } +} + +vector DBSCAN::calculateCluster(Point point) +{ + int index = 0; + vector::iterator iter; + vector clusterIndex; + for( iter = m_points.begin(); iter != m_points.end(); ++iter) + { + if ( calculateDistance(point, *iter) <= m_epsilon ) + { + clusterIndex.push_back(index); + } + index++; + } + return clusterIndex; +} + +inline double DBSCAN::calculateDistance(const Point& pointCore, const Point& pointTarget ) +{ + return pow(pointCore.x - pointTarget.x,2)+pow(pointCore.y - pointTarget.y,2)+pow(pointCore.z - pointTarget.z,2); +} + diff --git a/ros2/src/mfe_perception/lidar_cone_detector/src/file_loader.cpp b/ros2/src/mfe_perception/lidar_cone_detector/src/file_loader.cpp index 182a6cf..88bc73f 100644 --- a/ros2/src/mfe_perception/lidar_cone_detector/src/file_loader.cpp +++ b/ros2/src/mfe_perception/lidar_cone_detector/src/file_loader.cpp @@ -1,4 +1,4 @@ -#include +#include "../include/lidar_cone_detector/file_loader.hpp" namespace fs = std::filesystem; @@ -74,7 +74,7 @@ void FileLoaderNode::stream_test_pointcloud() { this->point_cloud_pub->publish(msg); - RCLCPP_INFO(this->get_logger(), "Published point cloud from file: %s", file_path.c_str()); + // RCLCPP_INFO(this->get_logger(), "Published point cloud from file: %s", file_path.c_str()); current_file_index_ = (current_file_index_ + 1) % binary_files_.size(); } diff --git a/ros2/src/mfe_perception/lidar_cone_detector/src/ground_plane_removal.cpp b/ros2/src/mfe_perception/lidar_cone_detector/src/ground_plane_removal.cpp index b224f90..ae7a485 100644 --- a/ros2/src/mfe_perception/lidar_cone_detector/src/ground_plane_removal.cpp +++ b/ros2/src/mfe_perception/lidar_cone_detector/src/ground_plane_removal.cpp @@ -1,160 +1,174 @@ -#include - -namespace lidar_cone_detector { - -GroundPlaneRemovalNode::GroundPlaneRemovalNode(const rclcpp::NodeOptions &options) -: Node("ground_plane_removal_node", options) -{ - // this->declare_parameter("lidar_frame", "map"); - this->lidar_frame = this->get_parameter("lidar_frame").as_string(); - - rclcpp::QoS qos_profile = rclcpp::QoS(rclcpp::KeepLast(10)).reliability(rclcpp::ReliabilityPolicy::Reliable); - - // Subscribes to the general point cloud and publishes ground data and the rest in two separate streams - point_cloud_sub = this->create_subscription( - "pcl/input", rclcpp::SensorDataQoS(), - std::bind(&GroundPlaneRemovalNode::remove_ground_plane_callback, this, std::placeholders::_1) - ); - point_cloud_ground_pub = this->create_publisher( - "pcl/ground", - qos_profile - ); - point_cloud_objs_pub = this->create_publisher( - "pcl/objects", - qos_profile - ); - - // params defined in header file -} - -// copy of the main method from 02/2025 demo ransac -void GroundPlaneRemovalNode::remove_ground_plane_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) -{ - pcl::PointCloud::Ptr pcd(new pcl::PointCloud); - pcl::fromROSMsg(*msg, *pcd); - - // TODO: filter remove all points beyond a certain distance from the origin - - // Voxel downsampling - pcl::VoxelGrid sor; - sor.setInputCloud(pcd); - sor.setLeafSize(0.05f, 0.05f, 0.05f); // sets box size in which will only contain 1 point - // can change how many points are in each box with setMinimumPointsNumberPerVoxel - pcl::PointCloud::Ptr downsampled_pcd(new pcl::PointCloud); - sor.filter(*downsampled_pcd); - - // Passthrough filter for downsampling - // TODO: Delegate to preprocessing as first-step for improved performance - /* - pcl::PointCloud::Ptr downsampled_pcd (new pcl::PointCloud); - pcl::PassThrough pass; - pass.setInputCloud (pcd); - pass.setFilterFieldName ("z"); - pass.setFilterLimits (-1.0, 0.2); - pass.setNegative (true); - pass.filter (*downsampled_pcd); - */ - - // Remove statistical outliers - // pcl::StatisticalOutlierRemoval sor_outlier; - // sor_outlier.setInputCloud(downsampled_pcd); - // sor_outlier.setMeanK(50); // number of neighbours to analyze - // sor_outlier.setStddevMulThresh(1.0); // threshold of std devs away from mean that will be marked as outlier - // pcl::PointCloud::Ptr filtered_pcd(new pcl::PointCloud); - // sor_outlier.filter(*filtered_pcd); - - // Plane segmentation (RANSAC) - pcl::SACSegmentation seg; - seg.setOptimizeCoefficients(true); - seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); - seg.setMethodType(pcl::SAC_RANSAC); - seg.setAxis(Eigen::Vector3f(0, 0, 1)); // Forces a horizontal plane - seg.setMaxIterations(100); - seg.setDistanceThreshold(0.03); - seg.setInputCloud(downsampled_pcd); - - pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); - pcl::PointIndices::Ptr inliers(new pcl::PointIndices); - - seg.segment(*inliers, *coefficients); - - // Logging coefficient data - if (coefficients->values.size() >= 4) { - RCLCPP_DEBUG( - this->get_logger(), - "RANSAC plane coefficients: [a=%f, b=%f, c=%f, d=%f]", - coefficients->values[0], - coefficients->values[1], - coefficients->values[2], - coefficients->values[3] - ); // macro for logger - } else { - RCLCPP_WARN(this->get_logger(), "Not enough coefficients returned by the segmenter."); - } - - // Extract inliers and outliers - pcl::ExtractIndices extract; - pcl::PointCloud::Ptr inlier_cloud(new pcl::PointCloud); - pcl::PointCloud::Ptr outlier_cloud(new pcl::PointCloud); - - extract.setInputCloud(downsampled_pcd); - extract.setIndices(inliers); - extract.setNegative(false); - extract.filter(*inlier_cloud); // inliers: ground plane - extract.setNegative(true); - extract.filter(*outlier_cloud); // outliers: everything else - - // Output processed data and publish to topics - sensor_msgs::msg::PointCloud2 cones_output; - sensor_msgs::msg::PointCloud2 ground_output; - - pcl::toROSMsg(*outlier_cloud, cones_output); - pcl::toROSMsg(*inlier_cloud, ground_output); - - // Set times and frame_ids, same as incoming msg - cones_output.header = msg->header; - ground_output.header = msg->header; - - this->point_cloud_objs_pub->publish(cones_output); - this->point_cloud_ground_pub->publish(ground_output); - -} // void remove_ground_plane_callback - -// DEPRECATED WARNING: CURRENT SOFTWARE STACK INCOMPATIBLE WITH RVIZ2 -/* - Performs visualization of the RANSAC algorithm using RViz2. - - Note: this function should only execute when ROS2 Parameter is setup properly. - - @param pcd pcl::PointerCloud The point cloud - @param vehicle_position Eigen::Vector3f The 3D coordinate of the vehicle from SLAM -*/ -void GroundPlaneRemovalNode::visualize(const pcl::PointCloud::Ptr &pcd, const Eigen::Vector3f &vehicle_position) -{ - pcl::visualization::PCLVisualizer viewer("Point Cloud Viewer"); - - // Add point cloud to the viewer - viewer.addPointCloud(pcd, "cloud"); - - // Set up the viewer - viewer.setBackgroundColor(0.0, 0.0, 0.0); - viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud"); - - // Add coordinate frame at the vehicle position - viewer.addCoordinateSystem(1.0, vehicle_position.x(), vehicle_position.y(), vehicle_position.z()); - - // Main loop for visualization - while (!viewer.wasStopped()) - { - viewer.spinOnce(100); - } -} // void visualize -} - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); - rclcpp::shutdown(); - return 0; +#include "../include/lidar_cone_detector/ground_plane_removal.hpp" + +namespace lidar_cone_detector { + +GroundPlaneRemovalNode::GroundPlaneRemovalNode(const rclcpp::NodeOptions &options) +: Node("ground_plane_removal_node", options) +{ + this->declare_parameter("lidar_frame", "map"); + this->lidar_frame = this->get_parameter("lidar_frame").as_string(); + + rclcpp::QoS qos_profile = rclcpp::QoS(rclcpp::KeepLast(10)).reliability(rclcpp::ReliabilityPolicy::Reliable); + + // Subscribes to the general point cloud and publishes ground data and the rest in two separate streams + point_cloud_sub = this->create_subscription( + "pcl/input", rclcpp::SensorDataQoS(), + std::bind(&GroundPlaneRemovalNode::remove_ground_plane_callback, this, std::placeholders::_1) + ); + point_cloud_ground_pub = this->create_publisher( + "pcl/ground", + qos_profile + ); + point_cloud_objs_pub = this->create_publisher( + "pcl/objects", + rclcpp::SensorDataQoS() // Match to subscription + ); + + // params defined in header file +} + +// copy of the main method from 02/2025 demo ransac +void GroundPlaneRemovalNode::remove_ground_plane_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) +{ + pcl::PointCloud::Ptr pcd(new pcl::PointCloud); + pcl::fromROSMsg(*msg, *pcd); + + + std::vector idx; + pcl::removeNaNFromPointCloud(*pcd, *pcd, idx); + + // ROI crop + pcl::PassThrough pass; + pass.setInputCloud(pcd); + + pass.setFilterFieldName("x"); pass.setFilterLimits(0.0f, 60.0f); pass.filter(*pcd); + pass.setFilterFieldName("y"); pass.setFilterLimits(-20.0f, 20.0f); pass.filter(*pcd); + pass.setFilterFieldName("z"); pass.setFilterLimits(-2.0f, 3.0f); pass.filter(*pcd); + // // TODO: filter remove all points beyond a certain distance from the origin + + // Voxel downsampling + pcl::VoxelGrid sor; + sor.setInputCloud(pcd); + float leaf = 0.08f; // tune downwards later + + sor.setLeafSize(leaf, leaf, leaf); // sets box size in which will only contain 1 point + // can change how many points are in each box with setMinimumPointsNumberPerVoxel + pcl::PointCloud::Ptr downsampled_pcd(new pcl::PointCloud); + sor.filter(*downsampled_pcd); + + // Passthrough filter for downsampling + // TODO: Delegate to preprocessing as first-step for improved performance + /* + pcl::PointCloud::Ptr downsampled_pcd (new pcl::PointCloud); + pcl::PassThrough pass; + pass.setInputCloud (pcd); + pass.setFilterFieldName ("z"); + pass.setFilterLimits (-1.0, 0.2); + pass.setNegative (true); + pass.filter (*downsampled_pcd); + */ + + // Remove statistical outliers + // pcl::StatisticalOutlierRemoval sor_outlier; + // sor_outlier.setInputCloud(downsampled_pcd); + // sor_outlier.setMeanK(50); // number of neighbours to analyze + // sor_outlier.setStddevMulThresh(1.0); // threshold of std devs away from mean that will be marked as outlier + // pcl::PointCloud::Ptr filtered_pcd(new pcl::PointCloud); + // sor_outlier.filter(*filtered_pcd); + + // Plane segmentation (RANSAC) + pcl::SACSegmentation seg; + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setAxis(Eigen::Vector3f(0.f, 0.f, 1.f)); // prefer planes ~parallel to XY + seg.setEpsAngle(pcl::deg2rad(10.0f)); // allow ±10° tilt (tune) + seg.setMaxIterations(100); + seg.setDistanceThreshold(0.03); + seg.setInputCloud(downsampled_pcd); + + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); + + seg.segment(*inliers, *coefficients); + + // Logging coefficient data + if (coefficients->values.size() >= 4) { + RCLCPP_DEBUG( + this->get_logger(), + "RANSAC plane coefficients: [a=%f, b=%f, c=%f, d=%f]", + coefficients->values[0], + coefficients->values[1], + coefficients->values[2], + coefficients->values[3] + ); // macro for logger + } else { + RCLCPP_WARN(this->get_logger(), "Not enough coefficients returned by the segmenter."); + } + + // Extract inliers and outliers + pcl::ExtractIndices extract; + pcl::PointCloud::Ptr inlier_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr outlier_cloud(new pcl::PointCloud); + + extract.setInputCloud(downsampled_pcd); + extract.setIndices(inliers); + extract.setNegative(false); + extract.filter(*inlier_cloud); // inliers: ground plane + extract.setNegative(true); + extract.filter(*outlier_cloud); // outliers: everything else + + // Output processed data and publish to topics + sensor_msgs::msg::PointCloud2 cones_output; + sensor_msgs::msg::PointCloud2 ground_output; + + pcl::toROSMsg(*outlier_cloud, cones_output); + pcl::toROSMsg(*inlier_cloud, ground_output); + + // Set times and frame_ids, same as incoming msg + cones_output.header = msg->header; + ground_output.header = msg->header; + + this->point_cloud_objs_pub->publish(cones_output); + this->point_cloud_ground_pub->publish(ground_output); + +} // void remove_ground_plane_callback + +// DEPRECATED WARNING: CURRENT SOFTWARE STACK INCOMPATIBLE WITH RVIZ2 +/* + Performs visualization of the RANSAC algorithm using RViz2. + + Note: this function should only execute when ROS2 Parameter is setup properly. + + @param pcd pcl::PointerCloud The point cloud + @param vehicle_position Eigen::Vector3f The 3D coordinate of the vehicle from SLAM +*/ +void GroundPlaneRemovalNode::visualize(const pcl::PointCloud::Ptr &pcd, const Eigen::Vector3f &vehicle_position) +{ + pcl::visualization::PCLVisualizer viewer("Point Cloud Viewer"); + + // Add point cloud to the viewer + viewer.addPointCloud(pcd, "cloud"); + + // Set up the viewer + viewer.setBackgroundColor(0.0, 0.0, 0.0); + viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud"); + + // Add coordinate frame at the vehicle position + viewer.addCoordinateSystem(1.0, vehicle_position.x(), vehicle_position.y(), vehicle_position.z()); + + // Main loop for visualization + while (!viewer.wasStopped()) + { + viewer.spinOnce(100); + } +} // void visualize +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); + rclcpp::shutdown(); + return 0; } \ No newline at end of file diff --git a/ros2/src/mfe_perception/lidar_cone_detector/src/lidar_preprocessor.cpp b/ros2/src/mfe_perception/lidar_cone_detector/src/lidar_preprocessor.cpp index 783ff1f..d68efd7 100644 --- a/ros2/src/mfe_perception/lidar_cone_detector/src/lidar_preprocessor.cpp +++ b/ros2/src/mfe_perception/lidar_cone_detector/src/lidar_preprocessor.cpp @@ -1,4 +1,4 @@ -#include +#include "../include/lidar_cone_detector/lidar_preprocessor.hpp" namespace lidar_cone_detector {