26 lidar cone detector to python

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 "<package> <launch_file>"
+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

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()
+ament_package()

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 <memory>
+#include <string>
+#include <vector>
+
+#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 <typename T> 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<int> of point indices)
+  void clusterPoints_DBSCAN(const pcl::PointCloud<pcl::PointXYZ>& cloud,
+                            std::vector<std::vector<int>>& clusters) const;
+
+  void publishDebugClouds(const pcl::PointCloud<pcl::PointXYZ>& all_cluster_pts,
+                          const pcl::PointCloud<pcl::PointXYZ>& centres,
+                          const std_msgs::msg::Header& header);
+
+  // --- ROS I/O ---
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_cloud_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_clusters_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_centres_;
+  rclcpp::Publisher<mfe_msgs::msg::Cone>::SharedPtr            pub_cone_;
+  rclcpp::Publisher<mfe_msgs::msg::Track>::SharedPtr           pub_track_;
+
+  // --- Parameters ---
+  double eps_{0.5};        // clustering radius (meters)
+  int    min_pts_{3};      // minimum points per cluster
+};
+
+} // namespace lidar_cone_detector
+

ros2/src/mfe_perception/lidar_cone_detector/include/lidar_cone_detector/dbscan.h

@@ -0,0 +1,50 @@
+#ifndef DBSCAN_H
+#define DBSCAN_H
+
+#include <vector>
+#include <cmath>
+
+#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<Point> points){
+        m_minPoints = minPts;
+        m_epsilon = eps;
+        m_points = points;
+        m_pointSize = points.size();
+    }
+    ~DBSCAN(){}
+
+    int run();
+    vector<int> 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<Point> m_points;
+
+private:    
+    unsigned int m_pointSize;
+    unsigned int m_minPoints;
+    float m_epsilon;
+};
+
+#endif // DBSCAN_H

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

ros2/src/mfe_perception/lidar_cone_detector/package.xml

@@ -14,19 +14,21 @@
   <depend>sensor_msgs</depend>
   <depend>vision_msgs</depend>
   <depend>pcl_msgs</depend>
+  <depend>mfe_msgs</depend>
 
   <depend>rclcpp</depend>
   <depend>eigen</depend>
+  <depend>eigen3</depend>
   <depend>tf2</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_ros</depend>
   <depend>pcl_ros</depend>
   <depend>pcl_conversions</depend>
   <depend>pointcloud_to_laserscan</depend>
-
+  <depend>geometry_msgs</depend>
   <depend>rclpy</depend>
   <depend>python3-sklearn</depend>
-
+  
   <exec_depend>libpcl-common</exec_depend>
   <exec_depend>libpcl-features</exec_depend>
   <exec_depend>libpcl-filters</exec_depend>