[ { "path": "CITATION.cff", "content": "cff-version: 1.2.0\nmessage: \"If you use this software, please cite it as below.\"\nauthors:\n- family-names: \"Horváth\"\n given-names: \"Ernő\"\n orcid: \"https://orcid.org/0000-0001-5083-2073\"\n- family-names: \"Pozna\"\n given-names: \"Claudiu\"\n- family-names: \"Unger\"\n given-names: \"Miklós\"\n orcid: \"https://orcid.org/0000-0003-3518-1107\"\ntitle: \"urban_road_filter\"\nversion: 1.1.2\ndate-released: 2023-05-24\nurl: \"https://github.com/jkk-research/urban_road_filter\"\npreferred-citation:\n type: article\n authors:\n - family-names: \"Horváth\"\n given-names: \"Ernő\"\n orcid: \"https://orcid.org/0000-0001-5083-2073\"\n - family-names: \"Pozna\"\n given-names: \"Claudiu\"\n - family-names: \"Unger\"\n given-names: \"Miklós\"\n orcid: \"https://orcid.org/0000-0003-3518-1107\"\n doi: \"10.3390/s22010194\"\n journal: \"Sensors\"\n title: \"Real-Time LIDAR-Based Urban Road and Sidewalk Detection for Autonomous Vehicles\"\n year: 2022\n" }, { "path": "CMakeLists.txt", "content": "cmake_minimum_required(VERSION 3.8)\nproject(urban_road_filter)\n\n# Default to C++17\nif(NOT CMAKE_CXX_STANDARD)\n set(CMAKE_CXX_STANDARD 17)\nendif()\n\n# Find required dependencies\nfind_package(ament_cmake REQUIRED)\nfind_package(rclcpp REQUIRED)\nfind_package(rclcpp_components REQUIRED)\nfind_package(sensor_msgs REQUIRED)\nfind_package(std_msgs REQUIRED)\nfind_package(visualization_msgs REQUIRED)\nfind_package(PCL REQUIRED)\nfind_package(pcl_conversions REQUIRED)\nfind_package(pcl_ros REQUIRED)\n\n# Include directories\ninclude_directories(\n include\n ${PCL_INCLUDE_DIRS}\n)\n\n# Add compile options\nadd_compile_options(-Wall -Wextra -O2)\nlink_directories(${PCL_LIBRARY_DIRS})\nadd_definitions(${PCL_DEFINITIONS})\n\n# Create the executable\nadd_executable(urban_road_filter \n src/lidar_segmentation.cpp \n src/main.cpp \n src/star_shaped_search.cpp \n src/x_zero_method.cpp \n src/z_zero_method.cpp \n src/blind_spots.cpp\n)\n\n# Link the executable\nament_target_dependencies(urban_road_filter \n rclcpp\n sensor_msgs\n std_msgs\n visualization_msgs\n pcl_conversions\n pcl_ros\n)\n\ntarget_link_libraries(urban_road_filter ${PCL_LIBRARIES})\n\n# Install the executable\ninstall(TARGETS\n urban_road_filter\n DESTINATION lib/${PROJECT_NAME})\n\n# Install launch files\ninstall(DIRECTORY\n launch\n DESTINATION share/${PROJECT_NAME})\n\n# Install config files\ninstall(DIRECTORY\n config\n DESTINATION share/${PROJECT_NAME})\n\n\nament_package()" }, { "path": "LICENSE", "content": "BSD 3-Clause License\n\nCopyright (c) 2021, JKK - Vehicle Industry Research Center\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n1. Redistributions of source code must retain the above copyright notice, this\n list of conditions and the following disclaimer.\n\n2. Redistributions in binary form must reproduce the above copyright notice,\n this list of conditions and the following disclaimer in the documentation\n and/or other materials provided with the distribution.\n\n3. Neither the name of the copyright holder nor the names of its\n contributors may be used to endorse or promote products derived from\n this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n" }, { "path": "README.md", "content": "# `urban_road_filter`: a real-time LIDAR-based urban road and sidewalk detection algorithm for autonomous vehicles\n\n \n\n# Dependency\n\n- [ROS 2](https://docs.ros.org/en/humble/index.html) (tested with [![Static Badge](https://img.shields.io/badge/ROS_2-Humble-34aec5)](https://docs.ros.org/en/humble/) and [![Static Badge](https://img.shields.io/badge/ROS_2-Jazzy-34aec5)](https://docs.ros.org/en/jazzy/))\n- [PCL](https://pointclouds.org/)\n\n# Install (download and build)\n\nUse the following commands to download and compile the package.\n\n```bash\ncd ~/ros2_ws/src\n```\n\n```bash\ngit clone https://github.com/jkk-research/urban_road_filter -b ros2\n```\n\nAnd build:\n\n```bash\ncd ~/ros2_ws\n```\n\n```bash\ncolcon build --packages-select urban_road_filter --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release\n```\n\n# Getting started\n\nIssue the following commands to start ROS 2, play sample data, and start the algorithm with visualization.\n\nIn a **new terminal** start ROS 2:\n\nDon't forget to source your workspace first\n
\nsource ~/ros2_ws/install/setup.bash\n
\n\n\n```bash\nros2 launch urban_road_filter urban_road_filter.launch.py\n```\n\n# Features\n\nThe `urban_road_filter` package provides the following features:\n\n1. **Detection Methods**:\n - `x_zero_method`: Detects roadside points by analyzing the angle between three points while keeping the X-coordinate constant.\n - `z_zero_method`: Detects roadside points by analyzing the angle between two vectors while keeping the Z-coordinate constant.\n - `star_shaped_method`: Uses a star-shaped algorithm to detect roadside points within a sector.\n\n2. **Configurable Parameters**:\n - Detection area dimensions (`min_x`, `max_x`, `min_y`, `max_y`, `min_z`, `max_z`).\n - LIDAR vertical resolution (`interval`).\n - Curb detection parameters (`curb_height`, `curb_points`).\n - Polygon simplification options (`polysimp_allow`, `polysimp`, `polyz`).\n\n3. **ROS Topics**:\n - `/road`: Filtered points representing the drivable road.\n - `/curb`: Filtered points representing curbs.\n - `/roi`: Filtered points within the region of interest.\n - `/road_marker`: Visualization markers for detected roads.\n\n4. **Performance Metrics**:\n - Publishes statistics about the segmentation results, including the number of points classified as road, curb, or non-road.\n\n# Configuration\n\nThe behavior of the `urban_road_filter` node can be customized using the `parameters.yaml` file. Key parameters include:\n\n- `fixed_frame`: The fixed frame for the LIDAR data.\n- `topic_name`: The topic to subscribe to for point cloud data.\n- `x_zero_method`, `z_zero_method`, `star_shaped_method`: Enable or disable specific detection methods.\n- `interval`: Acceptable interval for the LIDAR's vertical angular resolution.\n- `curb_height`, `curb_points`: Parameters for curb detection.\n- `polysimp_allow`, `polysimp`, `polyz`: Parameters for polygon simplification.\n\n# Cite & paper\n\nIf you use any of this code, please consider citing the [paper](https://www.mdpi.com/1424-8220/22/1/194):\n\n```bibtex\n@Article{roadfilt2022horv,\n title = {Real-Time LIDAR-Based Urban Road and Sidewalk Detection for Autonomous Vehicles},\n author = {Horváth, Ernő and Pozna, Claudiu and Unger, Miklós},\n journal = {Sensors},\n volume = {22},\n year = {2022},\n number = {1},\n url = {https://www.mdpi.com/1424-8220/22/1/194},\n issn = {1424-8220},\n doi = {10.3390/s22010194}\n}\n```\n\n# Related solutions\n\n- [`points_preprocessor`](https://github.com/Autoware-AI/core_perception/tree/master/points_preprocessor) `ray_ground_filter` and `ring_ground_filter` (ROS)\n- [`linefit_ground_segmentation`](https://github.com/lorenwel/linefit_ground_segmentation) (ROS)\n- [`curb_detection`](https://github.com/linyliny/curb_detection) (ROS)\n- [`3DLidar_curb_detection`](https://github.com/SohaibAl-emara/3D_Lidar_Curb_Detection) (ROS)\n- [`lidar_filter`](https://github.com/ZoltanTozser/lidar_filter)\n- Many more algorithms without code mentioned in the [paper](https://doi.org/10.3390/s22010194).\n\n# Videos and images\n\n[](https://www.youtube.com/watch?v=T2qi4pldR-E)\n[](https://www.youtube.com/watch?v=HHnj4VcbSy4)\n\n[](https://www.youtube.com/watch?v=9tdzo2AyaHM)\n[](https://www.youtube.com/watch?v=lp6q_QvWA-Y)\n\n\n\n\n\n# ROS publications / subscriptions\n\n```mermaid\nflowchart LR\n\nP[points]:::gray -->|sensor_msgs/PointCloud2| U([urban_road_filt
node]):::gray\nU --> |sensor_msgs/PointCloud2| A[curb]:::gray\nU --> |sensor_msgs/PointCloud2| B[road]:::gray \nU --> |sensor_msgs/PointCloud2| C[road_probably]:::gray\nU --> |sensor_msgs/PointCloud2| D[roi]:::gray\nU --> |visualization_msgs/MarkerArray| E[road_marker]:::gray\n\nclassDef light fill:#34aec5,stroke:#152742,stroke-width:2px,color:#152742 \nclassDef dark fill:#152742,stroke:#34aec5,stroke-width:2px,color:#34aec5\nclassDef white fill:#ffffff,stroke:#152742,stroke-width:2px,color:#152742\nclassDef gray fill:#f6f8fa,stroke:#152742,stroke-width:2px,color:#152742\nclassDef red fill:#ef4638,stroke:#152742,stroke-width:2px,color:#fff\n```\n" }, { "path": "config/parameters.yaml", "content": "# This file consolidates all parameters for the urban_road_filter package.\nurban_road_filter:\n ros__parameters:\n fixed_frame: \"left_os1/os1_lidar\" # Fixed frame. (Restart needed if it changes.)\n topic_name: \"/left_os1/os1_cloud_node/points\" # The desired topic to subscribe to.\n\n # Enabling individual detection methods.\n x_zero_method: true # x zero method.\n z_zero_method: true # z zero method.\n star_shaped_method: true # star shaped method.\n blind_spots: true # Filtering blind spots.\n\n # Blindspot detection (x-direction)\n xDirection: true # Added xDirection parameter with a valid boolean value.\n\n # LIDAR's vertical resolution\n interval: 0.18\n\n # Estimated minimum height of the curb (in meters).\n curb_height: 0.05\n\n # Estimated number of points on the curb.\n curb_points: 5\n\n # Width of the beam zone (in degrees).\n beamZone: 30\n\n # Size of the examined area.\n min_x: 0.0\n max_x: 30.0\n min_y: -10.0\n max_y: 10.0\n min_z: -3.0\n max_z: -1.0\n\n # Parameters for detection methods.\n cylinder_deg_x: 150.0 # Included angle of the examined triangle (three points) for x_zero_method (in degrees).\n cylinder_deg_z: 140.0 # Included angle of the examined triangle (two vectors) for z_zero_method (in degrees).\n curb_slope_deg: 50.0 # Radial threshold for star_shaped_method (in degrees).\n\n # Coefficients for CSP (Curvature Simplification Process).\n kdev_param: 1.225 # Dispersion coefficient.\n kdist_param: 2.0 # Distance coefficient.\n\n # Star-beam Filter\n starbeam_filter: false # Use rectangular beams for detection instead of the whole sectors (StarShaped).\n\n dmin_param: 10 # Minimum number of points for dispersion (CSP).\n\n # Polygon simplification settings (CSP).\n simple_poly_allow: true # Use simplified polygon (height data will be lost).\n poly_s_param: 0.7 # Coefficient of polygon simplification.\n poly_z_manual: -1.5 # Set a constant polygon height.\n poly_z_avg_allow: true # Set polygon height to average value." }, { "path": "config/urban_road_filter.rviz", "content": "Panels:\n - Class: rviz_common/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Status1\n - /Road PointCloud1\n - /Curb PointCloud1\n - /ROI PointCloud1\n - /Road Marker1\n Splitter Ratio: 0.5\n Tree Height: 725\n - Class: rviz_common/Selection\n Name: Selection\n - Class: rviz_common/Tool Properties\n Expanded:\n - /2D Goal Pose1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.5886790156364441\n - Class: rviz_common/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz_common/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: Curb PointCloud\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz_default_plugins/Grid\n Color: 160; 160; 164\n Enabled: true\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 10\n Reference Frame: \n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz_default_plugins/PointCloud2\n Color: 0; 255; 0\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 4096\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: Road PointCloud\n Position Transformer: XYZ\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.05000000074505806\n Style: Flat Squares\n Topic:\n Depth: 5\n Durability Policy: Volatile\n Filter size: 10\n History Policy: Keep Last\n Reliability Policy: Reliable\n Value: /road\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz_default_plugins/PointCloud2\n Color: 255; 0; 0\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 4096\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: Curb PointCloud\n Position Transformer: XYZ\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.05000000074505806\n Style: Flat Squares\n Topic:\n Depth: 5\n Durability Policy: Volatile\n Filter size: 10\n History Policy: Keep Last\n Reliability Policy: Reliable\n Value: /curb\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz_default_plugins/PointCloud2\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 3750\n Min Color: 0; 0; 0\n Min Intensity: 3\n Name: ROI PointCloud\n Position Transformer: XYZ\n Selectable: true\n Size (Pixels): 1\n Size (m): 0.009999999776482582\n Style: Points\n Topic:\n Depth: 5\n Durability Policy: Volatile\n Filter size: 10\n History Policy: Keep Last\n Reliability Policy: Reliable\n Value: /roi\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Class: rviz_default_plugins/MarkerArray\n Enabled: true\n Name: Road Marker\n Namespaces:\n \"\": true\n Topic:\n Depth: 5\n Durability Policy: Volatile\n History Policy: Keep Last\n Reliability Policy: Reliable\n Value: /road_marker\n Value: true\n Enabled: true\n Global Options:\n Background Color: 48; 48; 48\n Fixed Frame: left_os1/os1_lidar\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz_default_plugins/Interact\n Hide Inactive Objects: true\n - Class: rviz_default_plugins/MoveCamera\n - Class: rviz_default_plugins/Select\n - Class: rviz_default_plugins/FocusCamera\n - Class: rviz_default_plugins/Measure\n Line color: 128; 128; 0\n - Class: rviz_default_plugins/SetInitialPose\n Covariance x: 0.25\n Covariance y: 0.25\n Covariance yaw: 0.06853891909122467\n Topic:\n Depth: 5\n Durability Policy: Volatile\n History Policy: Keep Last\n Reliability Policy: Reliable\n Value: /initialpose\n - Class: rviz_default_plugins/SetGoal\n Topic:\n Depth: 5\n Durability Policy: Volatile\n History Policy: Keep Last\n Reliability Policy: Reliable\n Value: /goal_pose\n - Class: rviz_default_plugins/PublishPoint\n Single click: true\n Topic:\n Depth: 5\n Durability Policy: Volatile\n History Policy: Keep Last\n Reliability Policy: Reliable\n Value: /clicked_point\n Transformation:\n Current:\n Class: rviz_default_plugins/TF\n Value: true\n Views:\n Current:\n Class: rviz_default_plugins/Orbit\n Distance: 19.73822784423828\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Focal Point:\n X: 0\n Y: 0\n Z: 0\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 0.315398246049881\n Target Frame: \n Value: Orbit (rviz)\n Yaw: 3.1354026794433594\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 1016\n Hide Left Dock: false\n Hide Right Dock: false\n QMainWindow State: 000000ff00000000fd0000000400000000000002480000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000003d50000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: false\n Width: 1848\n X: 487\n Y: 130\n" }, { "path": "img/README.md", "content": "# Images to help understand the algoritm better\n\n![](rosgraph01.svg)\n![](urban_road_filter_explain01.svg)\n![](urban_road_filter_example01.png)\n![](urban_road_filter_example02.png)\n![](urban_road_filter_example02zoom.svg)\n\n\n" }, { "path": "include/urban_road_filter/data_structures.hpp", "content": "#pragma once\r\n\r\n/*Basic includes.*/\r\n#include \r\n#include \r\n#include \r\n#include \r\n#include \r\n#include \r\n#include \r\n#include \r\n\r\n/*Includes for ROS 2.*/\r\n#include \"rclcpp/rclcpp.hpp\"\r\n\r\n/*Includes for Markers.*/\r\n#include \"visualization_msgs/msg/marker.hpp\"\r\n#include \"visualization_msgs/msg/marker_array.hpp\"\r\n\r\n/*Includes for PCL.*/\r\n#include \r\n#include \r\n#include \r\n#include \r\n#include \r\n\r\n/*ROS 2 specific message types*/\r\n#include \"sensor_msgs/msg/point_cloud2.hpp\"\r\n\r\n/*ramer-douglas-peucker*/\r\n#include \r\n#include \r\n#include \r\n#include \r\n\r\n#include \"std_msgs/msg/float32_multi_array.hpp\"\r\n\r\nusing namespace boost::assign;\r\n\r\ntypedef boost::geometry::model::d2::point_xy xy;\r\n\r\nstruct Point2D{\r\n pcl::PointXYZI p;\r\n float d;\r\n float alpha;\r\n short isCurbPoint;\r\n};\r\n\r\nstruct Point3D:public Point2D{\r\n float newY;\r\n};\r\n\r\nstruct polar //polar-coordinate struct for the points\r\n{\r\n int id; //original ID of point (from input cloud)\r\n float r; //radial coordinate\r\n float fi; //angular coordinate (ccw angle from x-axis)\r\n};\r\n\r\nstruct box //struct for detection beams\r\n{\r\n std::vector p; //points within the beam's area\r\n //box *l, *r; //pointer to adjacent beams (currently not used)\r\n bool yx; //whether it is aligned more with the y-axis (than the x-axis)\r\n float o, d; //internal parameters (trigonometry)\r\n};\r\n\r\nnamespace params{\r\n extern std::string fixedFrame; /* Fixed Frame.*/\r\n extern std::string topicName; /* subscribed topic.*/\r\n extern bool x_zero_method, z_zero_method, star_shaped_method ; /*Methods of roadside detection*/\r\n extern bool blind_spots; /*Vakfolt javító algoritmus.*/\r\n extern int xDirection; /*A vakfolt levágás milyen irányú.*/\r\n extern float interval; /*A LIDAR vertikális szögfelbontásának, elfogadott intervalluma.*/\r\n extern float curbHeight; /*Becsült minimum szegély magasság.*/\r\n extern int curbPoints; /*A pontok becsült száma, a szegélyen.*/\r\n extern float beamZone; /*A vizsgált sugárzóna mérete.*/\r\n extern float angleFilter1; /*X = 0 érték mellett, három pont által bezárt szög.*/\r\n extern float angleFilter2; /*Z = 0 érték mellett, két vektor által bezárt szög.*/\r\n extern float angleFilter3; /*Csaplár László kódjához szükséges. Sugár irányú határérték (fokban).*/\r\n extern float min_X, max_X, min_Y, max_Y, min_Z, max_Z; /*A vizsgált terület méretei.*/\r\n extern float kdev_param; //(see below)\r\n extern float kdist_param; //(see below)\r\n extern bool starbeam_filter; //Toggle usage of rectangular beams for starshaped algorithm instead of the whole sector (containing the beam)\r\n extern int dmin_param; //(see below)\r\n extern bool polysimp_allow; /*polygon-eygszerűsítés engedélyezése*/\r\n extern bool zavg_allow; /*egyszerűsített polygon z-koordinátái átlagból (engedély)*/\r\n extern float polysimp; /*polygon-egyszerűsítési tényező (Ramer-Douglas-Peucker)*/\r\n extern float polyz; /*manuálisan megadott z-koordináta (polygon)*/\r\n};\r\n/*For pointcloud filtering*/\r\ntemplate \r\nclass FilteringCondition : public pcl::ConditionBase\r\n{\r\npublic:\r\n typedef std::shared_ptr> Ptr;\r\n typedef std::shared_ptr> ConstPtr;\r\n typedef std::function FunctorT;\r\n\r\n FilteringCondition(FunctorT evaluator): \r\n pcl::ConditionBase(),_evaluator( evaluator ) \r\n {}\r\n\r\n virtual bool evaluate (const PointT &point) const {\r\n // just delegate ALL the work to the injected std::function\r\n return _evaluator(point);\r\n }\r\nprivate:\r\n FunctorT _evaluator;\r\n};\r\n\r\nclass Detector : public rclcpp::Node {\r\npublic:\r\n Detector();\r\n void initialize();\r\n\r\n int partition(std::vector>& array3D, int arc, int low, int high);\r\n void quickSort(std::vector>& array3D, int arc, int low, int high);\r\n void filtered(const pcl::PointCloud &cloud);\r\n void starShapedSearch(std::vector& array2D);\r\n void beam_init();\r\n void xZeroMethod(std::vector>& array3D, int index, int* indexArray);\r\n void zZeroMethod(std::vector>& array3D, int index, int* indexArray);\r\n void blindSpots(std::vector>& array3D, int index, int* indexArray, float* maxDistance);\r\n \r\n // Additional methods for ROS 2\r\n void declare_and_get_parameters();\r\n rcl_interfaces::msg::SetParametersResult on_parameter_changed(const std::vector ¶meters);\r\n void update_global_params();\r\n\r\nprivate:\r\n // Parameter callback handle\r\n OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;\r\n \r\n // Parameter declarations\r\n std::string fixed_frame_;\r\n std::string topic_name_;\r\n bool x_zero_method_;\r\n bool z_zero_method_;\r\n bool star_shaped_method_;\r\n bool blind_spots_;\r\n bool xDirection_;\r\n double interval_;\r\n double curb_height_;\r\n int curb_points_;\r\n int beamZone_;\r\n double cylinder_deg_x_;\r\n double cylinder_deg_z_;\r\n double curb_slope_deg_;\r\n double min_x_;\r\n double max_x_;\r\n double min_y_;\r\n double max_y_;\r\n double min_z_;\r\n double max_z_;\r\n double kdev_param_;\r\n double kdist_param_;\r\n bool starbeam_filter_;\r\n int dmin_param_;\r\n bool simple_poly_allow_;\r\n double poly_s_param_;\r\n bool poly_z_avg_allow_;\r\n double poly_z_manual_;\r\n \r\n // Publishers\r\n rclcpp::Publisher::SharedPtr pub_road; \r\n rclcpp::Publisher::SharedPtr pub_high; \r\n rclcpp::Publisher::SharedPtr pub_box; \r\n rclcpp::Publisher::SharedPtr pub_pobroad; \r\n rclcpp::Publisher::SharedPtr pub_marker;\r\n rclcpp::Publisher::SharedPtr pub_stats; // Statistics publisher\r\n\r\n // Subscriber\r\n rclcpp::Subscription::SharedPtr sub;\r\n \r\n // Callback function for point cloud processing\r\n void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg);\r\n\r\n // For Ramer-Douglas-Peucker algorithm\r\n boost::geometry::model::linestring line;\r\n boost::geometry::model::linestring simplified;\r\n};\r\n" }, { "path": "launch/urban_road_filter.launch.py", "content": "from launch import LaunchDescription\nfrom launch_ros.actions import Node\nfrom launch.actions import DeclareLaunchArgument\nfrom launch.substitutions import LaunchConfiguration\nimport os\nfrom ament_index_python.packages import get_package_share_directory\n\ndef generate_launch_description():\n # Get the package directory\n pkg_dir = get_package_share_directory('urban_road_filter')\n \n # Path to the parameters.yaml file\n config_file_path = os.path.join(pkg_dir, 'config', 'parameters.yaml')\n \n # Create the default rviz config path\n default_rviz_config_path = os.path.join(pkg_dir, 'config', 'urban_road_filter.rviz')\n \n # Create launch configuration variables\n use_sim_time = LaunchConfiguration('use_sim_time', default='true') # Changed to true for bag playback\n rviz_config_file = LaunchConfiguration('rviz_config_file', default=default_rviz_config_path)\n \n # Declare the launch arguments \n declare_use_sim_time_cmd = DeclareLaunchArgument(\n 'use_sim_time',\n default_value='true', # Changed to true for bag playback\n description='Use simulation (Gazebo) clock if true')\n \n declare_rviz_config_file_cmd = DeclareLaunchArgument(\n 'rviz_config_file',\n default_value=default_rviz_config_path,\n description='Full path to the RVIZ config file to use')\n \n # Start the urban_road_filter node\n urban_road_filter_cmd = Node(\n package='urban_road_filter',\n executable='urban_road_filter',\n name='urban_road_filter',\n output='screen',\n parameters=[config_file_path]\n )\n \n # Start RViz\n rviz_cmd = Node(\n package='rviz2',\n executable='rviz2',\n name='rviz2',\n arguments=['-d', rviz_config_file],\n output='screen'\n )\n \n # Make sure the rviz configuration directory exists\n os.makedirs(os.path.join(pkg_dir, 'config'), exist_ok=True)\n \n # Create the launch description and populate\n ld = LaunchDescription()\n \n # Add launch arguments\n ld.add_action(declare_use_sim_time_cmd)\n ld.add_action(declare_rviz_config_file_cmd)\n \n # Add nodes to the launch description\n ld.add_action(urban_road_filter_cmd)\n ld.add_action(rviz_cmd)\n \n return ld" }, { "path": "package.xml", "content": "\n\n\n urban_road_filter\n 1.0.0\n ROS 2 Real-Time LIDAR-based urban road and sidewalk detection algorithm for autonomous vehicles\n\n Barham Farraj\n AbdelrahmanAlabdallah\n Ernő Horváth\n Claudiu Pozna\n Miklós Unger\n \n MIT\n \n https://github.com/jkk-research/urban_road_filter\n https://doi.org/10.3390/s22010194\n\n ament_cmake\n \n rclcpp\n sensor_msgs\n std_msgs\n visualization_msgs\n pcl_conversions\n pcl_ros\n rclcpp_components\n libpcl-all-dev\n \n ament_lint_auto\n ament_lint_common\n\n \n ament_cmake\n \n\n" }, { "path": "src/blind_spots.cpp", "content": "#include \"urban_road_filter/data_structures.hpp\"\n\nint params::xDirection; //direction of the blindspot cutoff\nbool params::blind_spots; //enable blindspot correction\nfloat params::beamZone; //size of hte examined beamzone\n\nvoid Detector::blindSpots(std::vector>& array3D,int index,int* indexArray,float* maxDistance){\n /*Blind spot detection:\n We examine the second arc. (First one gives inaccurate results.)\n The intervals (segments) [90°-180° --- 180°-270°] and [0°-90° --- 270°-360°] are of greatest significance (at the two sides of the car).\n We search for 2 pieces of high points in both intervals.\n If there are two high points on the first arc in the interval, then the area between has a high chance of being a blind spot.*/\n float q1 = 0, q2 = 180, q3 = 180, q4 = 360; //the four segments (quarters) of the arc\n int i,j,k,l; //\"temporary\" variables\n\n if (params::blind_spots)\n {\n for (i = 0; i < indexArray[1]; i++)\n {\n if(array3D[1][i].isCurbPoint==2)\n {\n if (array3D[1][i].alpha >= 0 && array3D[1][i].alpha < 90)\n {\n if (array3D[1][i].alpha > q1)\n {\n q1 = array3D[1][i].alpha;\n }\n }\n else if (array3D[1][i].alpha >= 90 && array3D[1][i].alpha < 180)\n {\n if (array3D[1][i].alpha < q2)\n {\n q2 = array3D[1][i].alpha;\n }\n }\n else if (array3D[1][i].alpha >= 180 && array3D[1][i].alpha < 270)\n {\n if (array3D[1][i].alpha > q3)\n {\n q3 = array3D[1][i].alpha;\n }\n }\n else\n {\n if (array3D[1][i].alpha < q4)\n {\n q4 = array3D[1][i].alpha;\n }\n }\n }\n }\n }\n\n float arcDistance; //arc length at the given angle - It is important to use the same arc length to examine every arc.\n int notRoad; //If there is a high point in the given segment on the given arc, then value 1 will be assigned to it, 0 otherwise.\n int blindSpot; //blind spots by the car\n float currentDegree; //the angle on the current arc\n\n /*determining arc length*/\n arcDistance = ((maxDistance[0] * M_PI) / 180) * params::beamZone;\n\n /*from 0° to [360° - beamZone]*/\n for (i = 0; i <= 360 - params::beamZone; i++)\n {\n blindSpot = 0;\n\n if (params::blind_spots)\n {\n /*If these conditions are met, then we have reached a blind spot and we stop checking.*/\n if (params::xDirection == 0)\n {\n /*evaluating the points in both directions (+-X)*/\n if ((q1 != 0 && q4 != 360 && (i <= q1 || i >= q4)) || (q2 != 180 && q3 != 180 && i >= q2 && i <= q3))\n {\n blindSpot = 1;\n }\n }\n else if (params::xDirection == 1)\n {\n /*evaluating the points in +X direction.*/\n if ((q2 != 180 && i >= q2 && i <= 270) || (q1 != 0 && (i <= q1 || i >= 270)))\n {\n blindSpot = 1;\n }\n }\n else\n {\n /*evaluating the points in -X direction.*/\n if ((q4 != 360 && (i >= q4 || i <= 90)) || (q3 != 180 && i <= q3 && i >= 90))\n {\n blindSpot = 1;\n }\n }\n }\n\n if (blindSpot == 0)\n {\n /*By default settings there's no high point in the given segment.*/\n notRoad = 0;\n\n /*evaluation of the given segment of the first arc*/\n for (j = 0; array3D[0][j].alpha <= i + params::beamZone && j < indexArray[0]; j++)\n {\n if (array3D[0][j].alpha >= i)\n {\n /*The segment needs no further checking if a high point is found.*/\n if (array3D[0][j].isCurbPoint == 2)\n {\n notRoad = 1;\n break;\n }\n }\n }\n\n /*If no high point is found in the given segment of the first arc, we can proceed to the next arc.*/\n if (notRoad == 0)\n {\n /*We accept the segment of the first arc.*/\n for (j = 0; array3D[0][j].alpha <= i + params::beamZone && j < indexArray[0]; j++)\n {\n if (array3D[0][j].alpha >= i)\n {\n array3D[0][j].isCurbPoint = 1;\n }\n }\n\n /*checking the rest of the arcs*/\n for (k = 1; k < index; k++)\n {\n /*A new angle needs to be defined to get the same arc length at every radius.*/\n if (i == 360 - params::beamZone)\n {\n currentDegree = 360;\n }\n else\n {\n currentDegree = i + arcDistance / ((maxDistance[k] * M_PI) / 180);\n }\n\n /*checking the points of the new arc*/\n for (l = 0; array3D[k][l].alpha <= currentDegree && l < indexArray[k]; l++)\n {\n if (array3D[k][l].alpha >= i)\n {\n /*No further processing is needed if a high point is found within the segment.*/\n if (array3D[k][l].isCurbPoint == 2)\n {\n notRoad = 1;\n break;\n }\n }\n }\n\n /*The rest of the arcs do not need to be checked if the beam stops at a high point.*/\n if (notRoad == 1)\n break;\n\n /*else: the given segment of the given arc is accepted*/\n for (l = 0; array3D[k][l].alpha <= currentDegree && l < indexArray[k]; l++)\n {\n if (array3D[k][l].alpha >= i)\n {\n array3D[k][l].isCurbPoint = 1;\n }\n }\n }\n }\n }\n }\n\n /*same as before but we check from 360° to [0° + beamZone] this time*/\n for (i = 360; i >= 0 + params::beamZone; --i)\n {\n blindSpot = 0;\n\n if (params::blind_spots)\n {\n /*If these conditions are met, then we have reached a blind spot and we stop checking.*/\n if (params::xDirection == 0)\n {\n /*evaluating the points in both directions (+-X)*/\n if ((q1 != 0 && q4 != 360 && (i <= q1 || i >= q4)) || (q2 != 180 && q3 != 180 && i >= q2 && i <= q3))\n {\n blindSpot = 1;\n }\n }\n else if (params::xDirection == 1)\n {\n /*evaluating the points in +X direction.*/\n if ((q2 != 180 && i >= q2 && i <= 270) || (q1 != 0 && (i <= q1 || i >= 270)))\n {\n blindSpot = 1;\n }\n }\n else\n {\n /*evaluating the points in -X direction.*/\n if ((q4 != 360 && (i >= q4 || i <= 90)) || (q3 != 180 && i <= q3 && i >= 90))\n {\n blindSpot = 1;\n }\n }\n }\n\n if (blindSpot == 0)\n {\n /*By default settings there's no high point in the given segment.*/\n notRoad = 0;\n\n /*evaluation of the given segment of the first arc*/\n for (j = indexArray[0] - 1; array3D[0][j].alpha >= i - params::beamZone && j >= 0; --j)\n {\n if (array3D[0][j].alpha <= i)\n {\n /*The segment needs no further checking if a high point is found.*/\n if (array3D[0][j].isCurbPoint == 2)\n {\n notRoad = 1;\n break;\n }\n }\n }\n\n /*If no high point is found in the given segment of the first arc, we can proceed to the next arc.*/\n if (notRoad == 0)\n {\n /*We accept the segment of the first arc.*/\n for (j = indexArray[0] - 1; array3D[0][j].alpha >= i - params::beamZone && j >= 0; --j)\n {\n if (array3D[0][j].alpha <= i)\n {\n array3D[0][j].isCurbPoint = 1;\n }\n }\n\n /*checking the rest of the arcs*/\n for (k = 1; k < index; k++)\n {\n /*A new angle needs to be defined to get the same arc length at every radius.*/\n if (i == 0 + params::beamZone)\n {\n currentDegree = 0;\n }\n else\n {\n currentDegree = i - arcDistance / ((maxDistance[k] * M_PI) / 180);\n }\n\n /*checking the points of the new arc*/\n for (l = indexArray[k] - 1; array3D[k][l].alpha >= currentDegree && l >= 0; --l)\n {\n if (array3D[k][l].alpha <= i)\n {\n /*The segment needs no further processing if a high point is found.*/\n if (array3D[k][l].isCurbPoint == 2)\n {\n notRoad = 1;\n break;\n }\n }\n }\n\n /*The rest of the arcs do not need to be checked if the beam stops at a high point.*/\n if (notRoad == 1)\n break;\n\n /*else: the given segment of the given arc is accepted*/\n for (l = indexArray[k] - 1; array3D[k][l].alpha >= currentDegree && l >= 0; --l)\n {\n if (array3D[k][l].alpha <= i)\n {\n array3D[k][l].isCurbPoint = 1;\n }\n }\n }\n }\n }\n }\n}" }, { "path": "src/lidar_segmentation.cpp", "content": "#include \"urban_road_filter/data_structures.hpp\"\n\n/*Global variables.*/\nint channels = 64; //The number of channels of the LIDAR .\nstd::string params::fixedFrame; //Fixed Frame.\nstd::string params::topicName; //subscribed topic\nbool params::x_zero_method,\n params::z_zero_method,\n params::star_shaped_method; //methods of roadside detection\nfloat params::interval; //acceptable interval for the LIDAR's vertical angular resolution\nfloat params::min_X,\n params::max_X,\n params::min_Y,\n params::max_Y,\n params::min_Z,\n params::max_Z; //dimensions of detection area\n\nbool params::polysimp_allow = true; //enable polygon simplification\nbool params::zavg_allow = true; //enable usage of average 'z' value as polygon height\nfloat params::polysimp = 0.5; //coefficient of polygon simplification (ramer-douglas-peucker)\nfloat params::polyz = -1.5; //manually set z-coordinate (output polygon)\n\nint ghostcount = 0; //counter variable helping to remove obsolete markers (ghosts)\n\nvoid marker_init(visualization_msgs::msg::Marker& m)\n{\n m.pose.position.x = 0;\n m.pose.position.y = 0;\n m.pose.position.z = 0;\n\n m.pose.orientation.x = 0.0;\n m.pose.orientation.y = 0.0;\n m.pose.orientation.z = 0.0;\n m.pose.orientation.w = 1.0;\n\n m.scale.x = 0.5;\n m.scale.y = 0.5;\n m.scale.z = 0.5;\n}\n\ninline std_msgs::msg::ColorRGBA setcolor(float r, float g, float b, float a)\n{\n std_msgs::msg::ColorRGBA c;\n c.r = r;\n c.g = g;\n c.b = b;\n c.a = a;\n return c;\n}\n\nDetector::Detector() : rclcpp::Node(\"urban_road_filter\")\n{\n RCLCPP_INFO(this->get_logger(), \"Initializing %s\", this->get_name());\n\n // Declare parameters\n declare_and_get_parameters();\n\n // Update global parameters (kept for compatibility with existing code)\n update_global_params();\n \n // Initialize lidar processing components\n initialize();\n}\n\nvoid Detector::initialize()\n{\n /*Initialize publishers and subscribers*/\n /*subscribing to the given topic*/\n sub = this->create_subscription(\n params::topicName, \n rclcpp::SensorDataQoS(),\n std::bind(&Detector::pointCloudCallback, this, std::placeholders::_1));\n \n /*publishing filtered points*/\n pub_road = this->create_publisher(\"road\", 10);\n pub_high = this->create_publisher(\"curb\", 10);\n pub_box = this->create_publisher(\"roi\", 10); // ROI - region of interest\n pub_pobroad = this->create_publisher(\"road_probably\", 10);\n pub_marker = this->create_publisher(\"road_marker\", 10);\n pub_stats = this->create_publisher(\"statistics\", 10); // Statistics publisher\n\n Detector::beam_init();\n\n RCLCPP_INFO(this->get_logger(), \"Ready\");\n}\n\nvoid Detector::pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg)\n{\n RCLCPP_INFO(this->get_logger(), \"Received point cloud with %d points, frame_id: %s\", \n cloud_msg->width * cloud_msg->height, cloud_msg->header.frame_id.c_str());\n \n pcl::PointCloud cloud;\n pcl::fromROSMsg(*cloud_msg, cloud);\n filtered(cloud);\n}\n\nvoid Detector::declare_and_get_parameters() {\n // Declare all parameters with defaults\n this->declare_parameter(\"fixed_frame\", \"velodyne\");\n this->declare_parameter(\"topic_name\", \"/velodyne_points\");\n this->declare_parameter(\"x_zero_method\", true);\n this->declare_parameter(\"z_zero_method\", true);\n this->declare_parameter(\"star_shaped_method\", true);\n this->declare_parameter(\"blind_spots\", true);\n this->declare_parameter(\"xDirection\", true);\n this->declare_parameter(\"interval\", 1.0);\n this->declare_parameter(\"curb_height\", 0.03);\n this->declare_parameter(\"curb_points\", 5);\n this->declare_parameter(\"beamZone\", 15);\n this->declare_parameter(\"cylinder_deg_x\", 2.5);\n this->declare_parameter(\"cylinder_deg_z\", 2.5);\n this->declare_parameter(\"curb_slope_deg\", 15.0);\n this->declare_parameter(\"min_x\", 0.0);\n this->declare_parameter(\"max_x\", 35.0);\n this->declare_parameter(\"min_y\", -10.0);\n this->declare_parameter(\"max_y\", 10.0);\n this->declare_parameter(\"min_z\", -3.0);\n this->declare_parameter(\"max_z\", 3.0);\n this->declare_parameter(\"kdev_param\", 25.0);\n this->declare_parameter(\"kdist_param\", 0.8);\n this->declare_parameter(\"starbeam_filter\", true);\n this->declare_parameter(\"dmin_param\", 10);\n this->declare_parameter(\"simple_poly_allow\", true);\n this->declare_parameter(\"poly_s_param\", 0.5);\n this->declare_parameter(\"poly_z_avg_allow\", false);\n this->declare_parameter(\"poly_z_manual\", 0.0);\n\n // Get parameter values\n fixed_frame_ = this->get_parameter(\"fixed_frame\").as_string();\n topic_name_ = this->get_parameter(\"topic_name\").as_string();\n x_zero_method_ = this->get_parameter(\"x_zero_method\").as_bool();\n z_zero_method_ = this->get_parameter(\"z_zero_method\").as_bool();\n star_shaped_method_ = this->get_parameter(\"star_shaped_method\").as_bool();\n blind_spots_ = this->get_parameter(\"blind_spots\").as_bool();\n xDirection_ = this->get_parameter(\"xDirection\").as_bool();\n interval_ = this->get_parameter(\"interval\").as_double();\n curb_height_ = this->get_parameter(\"curb_height\").as_double();\n curb_points_ = this->get_parameter(\"curb_points\").as_int();\n beamZone_ = this->get_parameter(\"beamZone\").as_int();\n cylinder_deg_x_ = this->get_parameter(\"cylinder_deg_x\").as_double();\n cylinder_deg_z_ = this->get_parameter(\"cylinder_deg_z\").as_double();\n curb_slope_deg_ = this->get_parameter(\"curb_slope_deg\").as_double();\n min_x_ = this->get_parameter(\"min_x\").as_double();\n max_x_ = this->get_parameter(\"max_x\").as_double();\n min_y_ = this->get_parameter(\"min_y\").as_double();\n max_y_ = this->get_parameter(\"max_y\").as_double();\n min_z_ = this->get_parameter(\"min_z\").as_double();\n max_z_ = this->get_parameter(\"max_z\").as_double();\n kdev_param_ = this->get_parameter(\"kdev_param\").as_double();\n kdist_param_ = this->get_parameter(\"kdist_param\").as_double();\n starbeam_filter_ = this->get_parameter(\"starbeam_filter\").as_bool();\n dmin_param_ = this->get_parameter(\"dmin_param\").as_int();\n simple_poly_allow_ = this->get_parameter(\"simple_poly_allow\").as_bool();\n poly_s_param_ = this->get_parameter(\"poly_s_param\").as_double();\n poly_z_avg_allow_ = this->get_parameter(\"poly_z_avg_allow\").as_bool();\n poly_z_manual_ = this->get_parameter(\"poly_z_manual\").as_double();\n\n // Set up parameter callback\n param_callback_handle_ = this->add_on_set_parameters_callback(\n std::bind(&Detector::on_parameter_changed, this, std::placeholders::_1));\n}\n\nrcl_interfaces::msg::SetParametersResult Detector::on_parameter_changed(\n const std::vector ¶meters) {\n rcl_interfaces::msg::SetParametersResult result;\n result.successful = true;\n \n for (const auto ¶m : parameters) {\n if (param.get_name() == \"fixed_frame\") {\n fixed_frame_ = param.as_string();\n } else if (param.get_name() == \"topic_name\") {\n topic_name_ = param.as_string();\n } else if (param.get_name() == \"x_zero_method\") {\n x_zero_method_ = param.as_bool();\n } else if (param.get_name() == \"z_zero_method\") {\n z_zero_method_ = param.as_bool();\n } else if (param.get_name() == \"star_shaped_method\") {\n star_shaped_method_ = param.as_bool();\n } else if (param.get_name() == \"blind_spots\") {\n blind_spots_ = param.as_bool();\n } else if (param.get_name() == \"xDirection\") {\n xDirection_ = param.as_bool();\n } else if (param.get_name() == \"interval\") {\n interval_ = param.as_double();\n } else if (param.get_name() == \"curb_height\") {\n curb_height_ = param.as_double();\n } else if (param.get_name() == \"curb_points\") {\n curb_points_ = param.as_int();\n } else if (param.get_name() == \"beamZone\") {\n beamZone_ = param.as_int();\n } else if (param.get_name() == \"cylinder_deg_x\") {\n cylinder_deg_x_ = param.as_double();\n } else if (param.get_name() == \"cylinder_deg_z\") {\n cylinder_deg_z_ = param.as_double();\n } else if (param.get_name() == \"curb_slope_deg\") {\n curb_slope_deg_ = param.as_double();\n } else if (param.get_name() == \"min_x\") {\n min_x_ = param.as_double();\n } else if (param.get_name() == \"max_x\") {\n max_x_ = param.as_double();\n } else if (param.get_name() == \"min_y\") {\n min_y_ = param.as_double();\n } else if (param.get_name() == \"max_y\") {\n max_y_ = param.as_double();\n } else if (param.get_name() == \"min_z\") {\n min_z_ = param.as_double();\n } else if (param.get_name() == \"max_z\") {\n max_z_ = param.as_double();\n } else if (param.get_name() == \"kdev_param\") {\n kdev_param_ = param.as_double();\n } else if (param.get_name() == \"kdist_param\") {\n kdist_param_ = param.as_double();\n } else if (param.get_name() == \"starbeam_filter\") {\n starbeam_filter_ = param.as_bool();\n } else if (param.get_name() == \"dmin_param\") {\n dmin_param_ = param.as_int();\n } else if (param.get_name() == \"simple_poly_allow\") {\n simple_poly_allow_ = param.as_bool();\n } else if (param.get_name() == \"poly_s_param\") {\n poly_s_param_ = param.as_double();\n } else if (param.get_name() == \"poly_z_avg_allow\") {\n poly_z_avg_allow_ = param.as_bool();\n } else if (param.get_name() == \"poly_z_manual\") {\n poly_z_manual_ = param.as_double();\n }\n }\n \n // Update global parameters to match node parameters\n update_global_params();\n \n RCLCPP_INFO(this->get_logger(), \"Parameters updated for %s\", this->get_name());\n return result;\n}\n\nvoid Detector::update_global_params() {\n // Update global parameters (kept for compatibility with existing code)\n params::fixedFrame = fixed_frame_;\n params::topicName = topic_name_;\n params::x_zero_method = x_zero_method_;\n params::z_zero_method = z_zero_method_;\n params::star_shaped_method = star_shaped_method_;\n params::blind_spots = blind_spots_;\n params::xDirection = xDirection_;\n params::interval = interval_;\n params::curbHeight = curb_height_;\n params::curbPoints = curb_points_;\n params::beamZone = beamZone_;\n params::angleFilter1 = cylinder_deg_x_;\n params::angleFilter2 = cylinder_deg_z_;\n params::angleFilter3 = curb_slope_deg_;\n params::min_X = min_x_;\n params::max_X = max_x_;\n params::min_Y = min_y_;\n params::max_Y = max_y_;\n params::min_Z = min_z_;\n params::max_Z = max_z_;\n params::kdev_param = kdev_param_;\n params::kdist_param = kdist_param_;\n params::starbeam_filter = starbeam_filter_;\n params::dmin_param = dmin_param_;\n params::polysimp_allow = simple_poly_allow_;\n params::polysimp = poly_s_param_;\n params::zavg_allow = poly_z_avg_allow_;\n params::polyz = poly_z_manual_;\n}\n\n/*FUNCTIONS*/\n\n/*recursive, quick sorting function (1/2)*/\nint Detector::partition(std::vector>& array3D, int arc,int low, int high)\n{\n float pivot = array3D[arc][high].alpha;\n int i = (low - 1);\n for (int j = low; j <= high - 1; j++){\n if (array3D[arc][j].alpha < pivot){\n i++;\n std::swap(array3D[arc][i],array3D[arc][j]);\n }\n }\n std::swap(array3D[arc][i+1],array3D[arc][high]);\n return (i + 1);\n}\n\n/*recursive, quick sorting function (2/2)*/\nvoid Detector::quickSort(std::vector>& array3D, int arc, int low, int high)\n{\n if (low < high)\n {\n int pi = partition(array3D, arc, low, high);\n quickSort(array3D, arc, low, pi - 1);\n quickSort(array3D, arc, pi + 1, high);\n }\n}\n\nvoid Detector::filtered(const pcl::PointCloud &cloud){\n /*variables for the \"for\" loops*/\n size_t i, j, k; // Removed unused variable 'l'\n\n // Debug print to see if the filtered method is executed\n RCLCPP_INFO(this->get_logger(), \"Starting filtered processing with %ld input points\", cloud.size());\n auto start_time = std::chrono::high_resolution_clock::now();\n\n pcl::PointXYZI pt; //temporary variable for storing a point\n auto cloud_filtered_Box = std::make_shared>(cloud); //all points in the detection area\n pcl::PointCloud cloud_filtered_Road; //filtered points (driveable road)\n pcl::PointCloud cloud_filtered_ProbablyRoad; //filtered points (non-driveable road)\n pcl::PointCloud cloud_filtered_High; //filtered points (non-road)\n\n\n auto filterCondition = std::make_shared>(\n [=](const pcl::PointXYZI& point){\n return point.x >= params::min_X && point.x <= params::max_X &&\n point.y >= params::min_Y && point.y <= params::max_Y &&\n point.z >= params::min_Z && point.z <= params::max_Z &&\n point.x + point.y + point.z != 0;\n }\n );\n pcl::ConditionalRemoval condition_removal;\n condition_removal.setCondition(filterCondition);\n condition_removal.setInputCloud(cloud_filtered_Box);\n condition_removal.filter(*cloud_filtered_Box);\n\n /*number of points in the detection area*/\n size_t piece = cloud_filtered_Box->points.size();\n\n /*A minimum of 30 points are requested in the detection area to avoid errors.\n Also, there is not much point in evaluating less data than that.*/\n if (piece < 30){\n return;\n }\n\n std::vector array2D(piece);\n\n /*variable for storing the input for trigonometric functions*/\n float bracket;\n\n /*A 1D array containing the various angular resolutions.\n This equals to the number of LiDAR channels.\n It is important to fill it with 0 values.*/\n float angle[channels] = {0};\n\n /*This helps to fill the 1D array containing the angular resolutions.*/\n int index = 0;\n\n /*whether the given angle corresponds to a new arc*/\n int newCircle;\n\n /*filling the 2D array*/\n for (size_t i = 0; i < piece; i++){\n /*--- filling the first 4 columns ---*/\n array2D[i].p = cloud_filtered_Box->points[i];\n array2D[i].d = sqrt(pow(array2D[i].p.x, 2) + pow(array2D[i].p.y, 2) + pow(array2D[i].p.z, 2));\n\n /*--- filling the 5. column ---*/\n bracket = abs(array2D[i].p.z) / array2D[i].d;\n\n /*required because of rounding errors*/\n if (bracket < -1)\n bracket = -1;\n else if (bracket > 1)\n bracket = 1;\n\n /*calculation and conversion to degrees*/\n if (array2D[i].p.z < 0)\n {\n array2D[i].alpha = acos(bracket) * 180 / M_PI;\n }\n else{\n array2D[i].alpha = (asin(bracket) * 180 / M_PI) + 90;\n }\n\n /*setting the index*/\n /*Our basic assumption is that the angle corresponds to a new circle/arc.*/\n newCircle = 1;\n\n /*If this value has already occured (within the specified interval), then this is not a new arc.\n Which means that \"newCircle = 0\", we can exit the loop, no further processing required.*/\n for (j = 0; j < channels; j++)\n {\n if (angle[j] == 0)\n break;\n\n if (abs(angle[j] - array2D[i].alpha) <= params::interval)\n {\n newCircle = 0;\n break;\n }\n }\n\n /*If no such value is registered in the array, then it's a new circle/arc.*/\n if (newCircle == 1)\n {\n /*We cannot allow the program to halt with a segmentation fault error.\n If for any reason there would come to be more than 64 arcs/circles, an error would occur.*/\n if (index < channels)\n {\n angle[index] = array2D[i].alpha;\n index++;\n }\n }\n }\n /*calling starShapedSearch algorithm*/\n if (params::star_shaped_method)\n Detector::starShapedSearch(array2D);\n \n\n /*Sorting the angular resolutions by ascending order...\n The smallest will be the first arc, etc..*/\n std::sort(angle, angle + index);\n\n std::vector> array3D(channels,std::vector(piece));\n\n /*This is required to set up the row indices of\n the groups (\"channels\") containing the arcs.\n It is important to fill it with 0 values.*/\n int indexArray[channels] = {0};\n\n /*A 1D array. The values of points that have the greatest distance from the origo.*/\n float maxDistance[channels] = {0};\n\n /*variable helping to handle errors caused by wrong number of channels.*/\n int results;\n\n /*filling the 3D array*/\n for (size_t i = 0; i < piece; i++)\n {\n results = 0;\n\n /*selecting the desired arc*/\n for (j = 0; j < index; j++)\n {\n if (abs(angle[j] - array2D[i].alpha) <= params::interval)\n {\n results = 1;\n break;\n }\n }\n\n if (results == 1)\n {\n /*assigning values from the 2D array*/\n array3D[j][indexArray[j]].p = array2D[i].p;\n\n /*the known \"high\" points*/\n if (params::star_shaped_method)\n array3D[j][indexArray[j]].isCurbPoint = array2D[i].isCurbPoint;\n\n /*The only difference here is that the distance is calculated in 2D - with no regard to the 'z' value.*/\n array3D[j][indexArray[j]].d = sqrt(pow(array2D[i].p.x, 2) + pow(array2D[i].p.y, 2));\n\n /*filling the 5. column with the angular position of points, in degrees.*/\n bracket = (abs(array3D[j][indexArray[j]].p.x)) / (array3D[j][indexArray[j]].d);\n if (bracket < -1)\n bracket = -1;\n else if (bracket > 1)\n bracket = 1;\n\n if (array3D[j][indexArray[j]].p.x >= 0 && array3D[j][indexArray[j]].p.y <= 0)\n {\n array3D[j][indexArray[j]].alpha = asin(bracket) * 180 / M_PI;\n }\n else if (array3D[j][indexArray[j]].p.x >= 0 && array3D[j][indexArray[j]].p.y > 0)\n {\n array3D[j][indexArray[j]].alpha = 180 - (asin(bracket) * 180 / M_PI);\n }\n else if (array3D[j][indexArray[j]].p.x < 0 && array3D[j][indexArray[j]].p.y >= 0)\n {\n array3D[j][indexArray[j]].alpha = 180 + (asin(bracket) * 180 / M_PI);\n }\n else\n {\n array3D[j][indexArray[j]].alpha = 360 - (asin(bracket) * 180 / M_PI);\n }\n\n if (array3D[j][indexArray[j]].d > maxDistance[j])\n {\n maxDistance[j] = array3D[j][indexArray[j]].d;\n }\n\n indexArray[j]++;\n }\n }\n\n if(params::x_zero_method)\n Detector::xZeroMethod(array3D,index,indexArray);\n if(params::z_zero_method)\n Detector::zZeroMethod(array3D,index,indexArray);\n\n float d;\n\n /*-- step 2.: filtering road points --*/\n /*ordering the elements of the array by angle on every arc*/\n for (i = 0; i < index; i++){\n quickSort(array3D, i, 0, indexArray[i] - 1);\n }\n /*blindspot detection*/\n Detector::blindSpots(array3D,index,indexArray,maxDistance);\n\n /*-- step 3: searching for marker points - the farthest green point within the given angle --*/\n /*It contains the points of the marker. The first three columns contain the X - Y - Z coordinates\n and the fourth column contains value 0 or 1 depending on whether there is a point within the given angle that is not marked as road.*/\n float markerPointsArray[piece][4];\n float maxDistanceRoad; //the distance of the farthest green point within the given angle\n int cM = 0; //variable helping to fill the marker with points (c - counter, M - Marker)\n int ID1, ID2; //which arc does the point fall onto (ID1) and (ordinally) which point is it (ID2)\n int redPoints; //whether there is a high point in the examined segment or a point that has not been marked as either road or high point\n\n /*checking the points by 1 degree at a time*/\n for (i = 0; i <= 360; i++)\n {\n ID1 = -1;\n ID2 = -1;\n maxDistanceRoad = 0;\n redPoints = 0;\n\n /*iterating through all the points of all the arcs*/\n for (j = 0; j < index; j++)\n {\n for (k = 0; k < indexArray[j]; k++)\n {\n /*If a non-road point is found, then we break the loop, because there will not be a road point found later on and value 1 will be assigned to the variable \"redPoints\".*/\n if (array3D[j][k].isCurbPoint != 1 && array3D[j][k].alpha >= i && array3D[j][k].alpha < i + 1)\n {\n redPoints = 1;\n break;\n }\n\n /*checking the distance for the detected green point*/\n if (array3D[j][k].isCurbPoint == 1 && array3D[j][k].alpha >= i && array3D[j][k].alpha < i + 1)\n {\n d = sqrt(pow(0 - array3D[j][k].p.x, 2) + pow(0 - array3D[j][k].p.y, 2));\n\n if (d > maxDistanceRoad)\n {\n maxDistanceRoad = d;\n ID1 = j;\n ID2 = k;\n }\n }\n }\n /*The previous \"break\" was used to exit the current circle, this one will exit all of them and proceed to the next angle.*/\n if (redPoints == 1)\n break;\n }\n\n /*adding the marker points to the array*/\n if (ID1 != -1 && ID2 != -1)\n {\n markerPointsArray[cM][0] = array3D[ID1][ID2].p.x;\n markerPointsArray[cM][1] = array3D[ID1][ID2].p.y;\n markerPointsArray[cM][2] = array3D[ID1][ID2].p.z;\n markerPointsArray[cM][3] = redPoints;\n cM++;\n }\n }\n\n /*-- step 4.: filling the groups --*/\n for (i = 0; i < index; i++)\n {\n for (j = 0; j < indexArray[i]; j++)\n {\n pt = array3D[i][j].p;\n /*road points*/\n if (array3D[i][j].isCurbPoint == 1)\n cloud_filtered_Road.push_back(pt);\n\n /*high points*/\n else if (array3D[i][j].isCurbPoint == 2)\n cloud_filtered_High.push_back(pt);\n }\n }\n\n /*-- step 5.: setting up the marker --*/\n /*There need to be at least 3 points to connect, otherwise errors might occur.*/\n if (cM > 2)\n {\n /*There might be a case where points are in red-green-red (or the other way around) order next to each other.\n This is bad is because the green / red marker (line strip) in this case will only consist of 1 point.\n This is not recommended, every point needs to have a pair of the same color.\n If the 3. column of \"markerPointsArray\" has the value 1 then it belongs to the red line strip,\n otherwise it belongs to the green one.*/\n\n /*If the first point is green but the second one is red,\n then the first one will be added to the red line strip too.*/\n if (markerPointsArray[0][3] == 0 && markerPointsArray[1][3] == 1)\n markerPointsArray[0][3] = 1;\n\n /*If the last point is green but the second to last is red,\n then the last one will be added to the red line strip too.*/\n if (markerPointsArray[cM - 1][3] == 0 && markerPointsArray[cM - 2][3] == 1)\n markerPointsArray[cM - 1][3] = 1;\n\n /*If the first point is red but the second one is green,\n then the first one will be added to the green line strip too.*/\n if (markerPointsArray[0][3] == 1 && markerPointsArray[1][3] == 0)\n markerPointsArray[0][3] = 0;\n\n /*If the last point is red but the second to last is green,\n then the last one will be added to the green line strip too.*/\n if (markerPointsArray[cM - 1][3] == 1 && markerPointsArray[cM - 2][3] == 0)\n markerPointsArray[cM - 1][3] = 0;\n\n /*Here we iterate through all the points.\n If a green point gets between two red ones, then it will be added to the red line strip too.\n The first two and last two points are not checked - they were already set before.*/\n for (i = 2; i <= cM - 3; i++)\n {\n if (markerPointsArray[i][3] == 0 && markerPointsArray[i - 1][3] == 1 && markerPointsArray[i + 1][3] == 1)\n markerPointsArray[i][3] = 1;\n }\n\n /*Here we iterate through all the points.\n If a red point gets between two green ones, then it will be added to the green line strip too.\n The first two and last two points are not checked - they were already set before.*/\n for (i = 2; i <= cM - 3; i++)\n {\n if (markerPointsArray[i][3] == 1 && markerPointsArray[i - 1][3] == 0 && markerPointsArray[i + 1][3] == 0)\n markerPointsArray[i][3] = 0;\n }\n\n visualization_msgs::msg::MarkerArray ma; //a marker array containing the green / red line strips\n visualization_msgs::msg::Marker line_strip; //the current green or red section / line strip\n geometry_msgs::msg::Point point; //point to fill the line strip with\n float zavg = 0.0; //average z value (for the simplified polygon)\n\n int lineStripID = 0; //ID of the given line strip\n\n line_strip.header.frame_id = params::fixedFrame;\n line_strip.header.stamp = this->now();\n line_strip.type = visualization_msgs::msg::Marker::LINE_STRIP;\n line_strip.action = visualization_msgs::msg::Marker::ADD;\n\n /*We iterate through the points which will make up the marker.*/\n for (i = 0; i < cM; i++)\n {\n /*adding the given point to a \"geometry_msgs::msg::Point\" type variable*/\n point.x = markerPointsArray[i][0];\n point.y = markerPointsArray[i][1];\n point.z = markerPointsArray[i][2];\n zavg *= i;\n zavg += point.z;\n zavg /= i+1;\n\n /*Adding the first point to the current line strip.\n No conditions need to be met for the first point.*/\n if (i == 0)\n {\n line_strip.points.push_back(point);\n line += xy(point.x,point.y);\n }\n\n /*If the next point is from the same group (red or green) as the previous one\n then it will be added to the line strip aswell.*/\n else if (markerPointsArray[i][3] == markerPointsArray[i - 1][3])\n {\n line_strip.points.push_back(point);\n line += xy(point.x,point.y);\n\n /*In this \"else if\" section we will reach the last point and the last line strip will be created.*/\n if (i == cM - 1)\n {\n line_strip.id = lineStripID;\n marker_init(line_strip);\n\n /*setting the color of the line strip*/\n if (markerPointsArray[i][3] == 0)\n {\n line_strip.color = setcolor(0.0, 1.0, 0.0, 1.0); //green\n }\n else\n {\n line_strip.color = setcolor(1.0, 0.0, 0.0, 1.0); //red\n }\n \n if (params::polysimp_allow)\n {\n line_strip.points.clear();\n boost::geometry::clear(simplified);\n boost::geometry::simplify(line, simplified, params::polysimp);\n for(boost::geometry::model::linestring::const_iterator it = simplified.begin(); it != simplified.end(); it++)\n {\n geometry_msgs::msg::Point p;\n p.x = boost::geometry::get<0>(*it);\n p.y = boost::geometry::get<1>(*it);\n p.z = params::polyz;\n\n line_strip.points.push_back(p);\n }\n }\n\n ma.markers.push_back(line_strip); //adding the line strip to the marker array\n line_strip.points.clear(); //We clear the points from the last line strip as there's no need for them anymore.\n boost::geometry::clear(line);\n }\n }\n\n /*change of category: red -> green\n The line joining the two points is still red, so we add the point to the given line strip.*/\n else if (markerPointsArray[i][3] != markerPointsArray[i - 1][3] && markerPointsArray[i][3] == 0)\n {\n line_strip.points.push_back(point);\n line += xy(point.x,point.y);\n\n /*The following points belong to a new line strip - a red one is being made here.*/\n line_strip.id = lineStripID;\n lineStripID++;\n\n marker_init(line_strip);\n\n line_strip.color = setcolor(1.0, 0.0, 0.0, 1.0); //red\n\n if (params::polysimp_allow)\n {\n line_strip.points.clear();\n boost::geometry::clear(simplified);\n boost::geometry::simplify(line, simplified, params::polysimp);\n for(boost::geometry::model::linestring::const_iterator it = simplified.begin(); it != simplified.end(); it++)\n {\n geometry_msgs::msg::Point p;\n p.x = boost::geometry::get<0>(*it);\n p.y = boost::geometry::get<1>(*it);\n p.z = params::polyz;\n\n line_strip.points.push_back(p);\n }\n }\n\n ma.markers.push_back(line_strip); //adding the line strip to the marker array\n line_strip.points.clear(); //the points are not needed anymore\n boost::geometry::clear(line);\n line_strip.points.push_back(point); //This point is needed for the next line strip aswell, so we add it.\n line += xy(point.x,point.y);\n }\n\n /*change of category: green -> red\n First we set up the green line strip, then we add the last point to the red one aswell,\n since there is always a red line strip between a green and a red point.*/\n else if (markerPointsArray[i][3] != markerPointsArray[i - 1][3] && markerPointsArray[i][3] == 1)\n {\n /*the green marker*/\n line_strip.id = lineStripID;\n lineStripID++;\n\n marker_init(line_strip);\n\n line_strip.color = setcolor(0.0, 1.0, 0.0, 1.0); //green\n\n if (params::polysimp_allow)\n {\n line_strip.points.clear();\n boost::geometry::clear(simplified);\n boost::geometry::simplify(line, simplified, params::polysimp);\n for(boost::geometry::model::linestring::const_iterator it = simplified.begin(); it != simplified.end(); it++)\n {\n geometry_msgs::msg::Point p;\n p.x = boost::geometry::get<0>(*it);\n p.y = boost::geometry::get<1>(*it);\n p.z = params::polyz;\n\n line_strip.points.push_back(p);\n }\n }\n\n ma.markers.push_back(line_strip); //adding the line strip to the marker array\n line_strip.points.clear(); //These points are not needed anymore.\n boost::geometry::clear(line);\n\n /*The previous point is required for the next line strip aswell.*/\n point.x = markerPointsArray[i - 1][0];\n point.y = markerPointsArray[i - 1][1];\n point.z = markerPointsArray[i - 1][2];\n line_strip.points.push_back(point);\n line += xy(point.x,point.y);\n\n /*The current point is required for the next line strip aswell.*/\n point.x = markerPointsArray[i][0];\n point.y = markerPointsArray[i][1];\n point.z = markerPointsArray[i][2];\n line_strip.points.push_back(point);\n line += xy(point.x,point.y);\n }\n line_strip.lifetime = rclcpp::Duration(0, 0); // No timeout\n }\n if (params::zavg_allow)\n {\n for (int seg=0; seg < (int)ma.markers.size(); seg++)\n {\n for (int mz = 0; mz < (int)ma.markers[seg].points.size(); mz++) //setting the height of the polygon from the average height of points\n {\n ma.markers[seg].points[mz].z = zavg;\n }\n }\n }\n\n /*removal of obsolete markers*/\n line_strip.action = visualization_msgs::msg::Marker::DELETE;\n for (int del = lineStripID; delpublish(ma);\n }\n\n\n for (j = 0; j < indexArray[10]; j++){\n pt = array3D[10][j].p;\n cloud_filtered_ProbablyRoad.push_back(pt);\n }\n \n\n /*Road and High topic header*/\n cloud_filtered_Road.header = cloud.header;\n cloud_filtered_ProbablyRoad.header = cloud.header;\n cloud_filtered_High.header = cloud.header;\n cloud_filtered_Box->header = cloud.header;\n\n /*Convert PCL cloud to ROS message and publish*/\n sensor_msgs::msg::PointCloud2 cloud_msg_road;\n sensor_msgs::msg::PointCloud2 cloud_msg_high;\n sensor_msgs::msg::PointCloud2 cloud_msg_box;\n sensor_msgs::msg::PointCloud2 cloud_msg_prob;\n \n pcl::toROSMsg(cloud_filtered_Road, cloud_msg_road);\n pcl::toROSMsg(cloud_filtered_High, cloud_msg_high);\n pcl::toROSMsg(*cloud_filtered_Box, cloud_msg_box);\n pcl::toROSMsg(cloud_filtered_ProbablyRoad, cloud_msg_prob);\n \n cloud_msg_road.header.frame_id = params::fixedFrame;\n cloud_msg_high.header.frame_id = params::fixedFrame;\n cloud_msg_box.header.frame_id = params::fixedFrame;\n cloud_msg_prob.header.frame_id = params::fixedFrame;\n \n cloud_msg_road.header.stamp = this->now();\n cloud_msg_high.header.stamp = this->now();\n cloud_msg_box.header.stamp = this->now();\n cloud_msg_prob.header.stamp = this->now();\n\n /*publishing*/\n pub_road->publish(cloud_msg_road); //filtered points (driveable road)\n pub_high->publish(cloud_msg_high); //filtered points (non-driveable road)\n pub_box->publish(cloud_msg_box); //filtered points (non-road)\n pub_pobroad->publish(cloud_msg_prob); //filtered points (probably road)\n\n // Add detailed performance metrics at the end of the function, before publishing\n auto end_time = std::chrono::high_resolution_clock::now();\n auto duration = std::chrono::duration_cast(end_time - start_time).count();\n \n // Calculate percentages\n float road_percentage = (cloud.size() > 0) ? (cloud_filtered_Road.size() * 100.0 / cloud.size()) : 0;\n float probably_road_percentage = (cloud.size() > 0) ? (cloud_filtered_ProbablyRoad.size() * 100.0 / cloud.size()) : 0;\n float non_road_percentage = (cloud.size() > 0) ? (cloud_filtered_High.size() * 100.0 / cloud.size()) : 0;\n \n // Log statistics about the segmentation results\n RCLCPP_INFO(this->get_logger(), \"Point cloud segmentation completed in %ld ms\", duration);\n RCLCPP_INFO(this->get_logger(), \"Statistics: Input points: %ld\", cloud.size());\n RCLCPP_INFO(this->get_logger(), \"Road points: %ld (%.1f%%)\", \n cloud_filtered_Road.size(), road_percentage);\n RCLCPP_INFO(this->get_logger(), \"Probably road points: %ld (%.1f%%)\", \n cloud_filtered_ProbablyRoad.size(), probably_road_percentage);\n RCLCPP_INFO(this->get_logger(), \"Non-road points: %ld (%.1f%%)\",\n cloud_filtered_High.size(), non_road_percentage);\n \n // Publish statistics as a ROS topic\n std_msgs::msg::Float32MultiArray stats_msg;\n stats_msg.data = {\n static_cast(cloud.size()), // Total points\n static_cast(cloud_filtered_Road.size()), // Road points\n static_cast(cloud_filtered_ProbablyRoad.size()), // Probably road points\n static_cast(cloud_filtered_High.size()), // Non-road points\n road_percentage, // Road percentage\n probably_road_percentage, // Probably road percentage\n non_road_percentage, // Non-road percentage\n static_cast(duration) // Processing time (ms)\n };\n pub_stats->publish(stats_msg);\n\n pcl::PointCloud::Ptr cloud_xyz_Road (new pcl::PointCloud);\n pcl::copyPointCloud(cloud_filtered_Road, *cloud_xyz_Road);\n pcl::PointCloud::Ptr cloud_xyz_ProbablyRoad (new pcl::PointCloud);\n pcl::copyPointCloud(cloud_filtered_ProbablyRoad, *cloud_xyz_ProbablyRoad);\n pcl::PointCloud::Ptr cloud_xyz_High (new pcl::PointCloud);\n pcl::copyPointCloud(cloud_filtered_High, *cloud_xyz_High);\n}" }, { "path": "src/main.cpp", "content": "#include \"urban_road_filter/data_structures.hpp\"\n#include \"rclcpp/rclcpp.hpp\"\n\nint main(int argc, char **argv)\n{\n // Initialize ROS 2\n rclcpp::init(argc, argv);\n \n // Create detector node\n auto node = std::make_shared();\n \n // Spin the node\n rclcpp::spin(node);\n \n // Clean up\n rclcpp::shutdown();\n return 0;\n}" }, { "path": "src/star_shaped_search.cpp", "content": "/* starShapedSearch algorithm\n (by László Csaplár)\n\n description: a complementary algorithm for roadside detection, part of the \"urban_road_filter\" package\n*/\n#include \"urban_road_filter/data_structures.hpp\"\n\nint rep = 360; //number of detection beams (how many parts/sectors will the pointcloud be divided along the angular direction -> one beam per sector)\nfloat width = 0.2; //width of beams\nfloat Kfi; //internal parameter, for assigning the points to their corresponding sectors ( = 1 / [2pi/rep] = 1 / [angle between neighboring beams] )\nfloat params::angleFilter3; //\"slope\" parameter for edge detection (in degrees, given by 2 points, in radial direction/plane)\nfloat slope_param; //same but in radians\nfloat params::kdev_param; //coefficient: weighting the impact of deviation (difference) from average ( = overall sensitivity to changes)\nfloat params::kdist_param; //coefficient: weighting the impact of the distance between points ( = sensitivity for height error at close points)\nbool params::starbeam_filter; //toggle usage of rectangular beams for starshaped algorithm instead of the whole sector (containing the beam)\nint params::dmin_param; //minimal number of points required to begin adaptive evaluation\n\n\nstd::vector beams(rep); //beams\nstd::vector beamp(rep + 1); //pointers to the beams (+1 -> 0 AND 360)\n\nbool ptcmpr(polar a, polar b) //comparison by r-coordinates\n{\n return (a.r < b.r);\n}\n\nfloat slope(float x0, float y0, float x1, float y1) //get slope between 2 points given by x and y coordinates\n{\n return (y1 - y0) / (x1 - x0);\n}\n\nvoid Detector::beam_init() //beam initialization\n{\n {\n float fi, off = 0.5 * width; //temporary variables\n for (int i = 0; i < rep; i++) //for every beam...\n {\n fi = i * 2 * M_PI / rep; //angle of beam\n if (abs(tan(fi)) > 1) //\"slope\" of beam ( x <---> y )\n {\n beams[i].yx = true; //aligning more with y-direction\n beams[i].d = tan(0.5 * M_PI - fi); // = 1/tan(fi) [coefficient]\n beams[i].o = abs(off / sin(fi)); //projection of half beam-width in the x-direction (how far from its centerline is the edge of the beam in the x-dir)\n }\n else\n {\n beams[i].yx = false; //aligning more with x-direction\n beams[i].d = tan(fi); //coefficient\n beams[i].o = abs(off / cos(fi)); //y-axis projection of half beam-width\n }\n beamp[i] = &beams[i]; //initializing the pointers\n }\n }\n\n/*\n for (int i = 0, j = 1; j < rep; i++, j++) //initializing the pointers to adjacent beams (currently unused/not needed)\n {\n beams[i].l = &beams[j];\n beams[j].r = &beams[i];\n }\n beams[0].r = &beams[rep-1];\n beams[rep-1].l = &beams[0];\n*/\n\n Kfi = rep / (2 * M_PI); //should be 2pi/rep, but then we would have to divide by it every time - using division only once and then multiplying later on should be somewhat faster (?)\n}\n\nvoid beamfunc(const int tid, std::vector &array2D) //beam algorithm (filtering, sorting, edge-/roadside detection) - input: beam ID (ordinal position/\"which one\" by angle), pointcloud (as std::vector, see: 'array2D' of 'lidarSegmentation')\n{\n int i = 0, s = beams[tid].p.size(); //loop variables\n float c; //temporary variable to simplify things\n\n if (params::starbeam_filter)\n {\n if (beams[tid].yx) //filtering the points lying outside the area of the beam... (case 1/2, y-direction)\n {\n while (i < s) //iterating through points in the current sector (instead of a for loop - since 's' is not constant and 'i' needs to be incremented conditionally)\n {\n c = beams[tid].d * array2D[beams[tid].p[i].id].p.y; //x-coordinate of the beam's centerline at the point (at the \"height\" of its y-coordinate)\n if ((c - beams[tid].o) < array2D[beams[tid].p[i].id].p.x && array2D[beams[tid].p[i].id].p.x < (c + beams[tid].o)) //whether it is inside the beam (by checking only x values on the line/\"height\" of the point's y-coordinate: whether the [x-coordinate of the] point falls between the [x-coordinates of the] two sides/borders of the beam\n {\n i++; //okay, next one\n }\n else //if outside the area\n {\n beams[tid].p.erase(beams[tid].p.begin() + i); //remove point\n s--; //the size has shrunk because of the deletion of a point (and its place is taken by the next point, so 'i' does not need to be changed)\n }\n }\n }\n else //same but with x and y swapped (case 2/2, x-direction)\n {\n while (i < s)\n {\n c = beams[tid].d * array2D[beams[tid].p[i].id].p.x;\n if ((c - beams[tid].o) < array2D[beams[tid].p[i].id].p.y && array2D[beams[tid].p[i].id].p.y < (c + beams[tid].o))\n {\n i++;\n }\n else\n {\n beams[tid].p.erase(beams[tid].p.begin() + i);\n s--;\n }\n }\n }\n }\n\n std::sort(beams[tid].p.begin(), beams[tid].p.end(), ptcmpr); //sorting points by r-coordinate (radius)\n\n { //edge detection (edge of the roadside)\n if (s > 1) //for less than 2 points it would be pointless\n {\n float kdev = params::kdev_param; //coefficient: weighting the impact of deviation (difference) from average ( = overall sensitivity to changes)\n float kdist = params::kdist_param; //coefficient: weighting the impact of the distance between points ( = sensitivity for height error at close points)\n int dmin = params::dmin_param; //minimal number of points required to begin adaptive evaluation\n\n float avg = 0, dev = 0, nan = 0; //average value and absolute average deviation of the slope for adaptive detection + handling Not-a-Number values\n float ax, ay, bx, by, slp; //temporary variables (points 'a' and 'b' + slope)\n bx = beams[tid].p[0].r; //x = r-coordinate of the first point (radial position)\n by = array2D[beams[tid].p[0].id].p.z; //y = z-coordinate of the first point (height)\n\n for (int i = 1; i < s; i++) //edge detection based on the slope between point a and b\n { //updating points (a=b, b=next)\n ax = bx;\n bx = beams[tid].p[i].r;\n ay = by;\n by = array2D[beams[tid].p[i].id].p.z;\n slp = slope(ax, ay, bx, by);\n\n if (isnan(slp))\n nan++; //Not-a-Number correction\n else //calculating (updating) average and deviation\n {\n avg *= i - nan - 1; //\"unpacking\" average value (average -> sum, + NaN correction)\n avg += slp; //insertion of new element\n avg *= 1 / (i - nan); //sum -> average\n dev *= i - nan - 1; //Absolute Average Deviation -> sum (\"unpacking\")\n dev += abs(slp - avg); //insertion of new element (absolute difference from average)\n dev *= 1 / (i - nan); //sum -> AAD\n }\n if ( slp > slope_param || //evaluation of the slope -> using a constant + adaptive:\n (i > dmin && (slp * slp - avg * avg) * kdev * ((bx - ax) * kdist) > dev) //if sufficient number of points: does the (weighted) \"squared difference\" from average - corrected with... \n ) //... the (weighted) distance of adjacent points - exceed the average absolute deviation?\n {\n array2D[beams[tid].p[i].id].isCurbPoint = 2; //the point in the 2D array gets marked as curbpoint\n break; //(the roadside is found, time to break the loop)\n }\n }\n }\n }\n beams[tid].p.clear(); //evaluation done, the points are no longer needed\n}\n\nvoid Detector::starShapedSearch(std::vector &array2D) //entry point to the code, everything gets called here (except for initialization - that needs to be called separately, at the start of the program - \"beam_init()\")\n{\n beamp.push_back(&beams[0]); //initializing \"360 deg = 0 deg\" pointer\n int f, s = array2D.size(); //temporary variables\n float r, fi; //polar coordinates\n slope_param = params::angleFilter3 * (M_PI / 180);\n\n for (int i = 0; i < s; i++) //points to polar coordinate-system + sorting into sectors\n {\n r = sqrt(array2D[i].p.x * array2D[i].p.x + array2D[i].p.y * array2D[i].p.y); //r = sqRoot(x^2+y^2) = distance of point from sensor\n\n fi = atan2(array2D[i].p.y, array2D[i].p.x); //angular position of point\n\n if (fi < 0)\n fi += 2 * M_PI; //handling negative values (-180...+180 -> 0...360)\n\n f = (int)(fi * Kfi); //which one of the beams (sectors, containing the beam) does it fall into\n\n beamp[f]->p.push_back(polar{i, r, fi}); //adding the point to the 'f'-th beam (still unfiltered)\n }\n beamp.pop_back(); //removing pointer (to prevent \"double free\" error)\n\n for (int i = 0; i < rep; i++) //for every beam...\n {\n beamfunc(i, array2D); //the heart of the starshaped method (beam algorithm)\n }\n}\n" }, { "path": "src/x_zero_method.cpp", "content": "#include \"urban_road_filter/data_structures.hpp\"\n\nfloat params::angleFilter1; //angle given by three points, while keeping X = 0\nfloat params::curbHeight; //estimated minimal height of the curb\nint params::curbPoints; //estimated number of points on the curb\n\nvoid Detector::xZeroMethod(std::vector>& array3D,int index,int* indexArray){\n /*-- step 1.: filtering the NON-road points --*/\n int p2, p3; //2nd and 3rd of the points that are being examined\n\n /*\n --> alpha - angle between the three points and the two vectors\n --> x1, x2, x3 - length of the sides of the triangle given by the three points\n --> curbPoints - estimated number of points on the roadside\n --> d - distance between the two outermost points - the rotation of the LiDAR and arc gaps make it necessary\n d - this variable also gets used later on in the code\n */\n float alpha, x1, x2, x3, d, bracket;\n for (int i = 0; i < index; i++)\n {\n /*assigning new Y values while keeping X = 0 */\n for (int j = 1; j < indexArray[i]; j++)\n {\n array3D[i][j].newY = array3D[i][j-1].newY + 0.0100;\n }\n\n /*evaluation of the points in an arc - x-zero method*/\n for (int j = params::curbPoints; j <= (indexArray[i] - 1) - params::curbPoints; j++)\n {\n p2 = j + params::curbPoints / 2;\n p3 = j + params::curbPoints;\n\n d = sqrt(\n pow(array3D[i][p3].p.x - array3D[i][j].p.x , 2) +\n pow(array3D[i][p3].p.y - array3D[i][j].p.y , 2));\n\n /*set the distance to be less than 5 meters*/\n if (d < 5.0000)\n {\n x1 = sqrt(\n pow(array3D[i][p2].newY - array3D[i][j].newY , 2) +\n pow(array3D[i][p2].p.z - array3D[i][j].p.z , 2));\n x2 = sqrt(\n pow(array3D[i][p3].newY - array3D[i][p2].newY, 2) +\n pow(array3D[i][p3].p.z - array3D[i][p2].p.z , 2));\n x3 = sqrt(\n pow(array3D[i][p3].newY - array3D[i][j].newY , 2) +\n pow(array3D[i][p3].p.z - array3D[i][j].p.z , 2));\n\n bracket = (pow(x3, 2) - pow(x1, 2) - pow(x2, 2)) / (-2 * x1 * x2);\n if (bracket < -1)\n bracket = -1;\n else if (bracket > 1)\n bracket = 1;\n\n alpha = acos(bracket) * 180 / M_PI;\n\n /*condition and assignment to group*/\n if (alpha <= params::angleFilter1 &&\n (abs(array3D[i][j].p.z - array3D[i][p2].p.z ) >= params::curbHeight ||\n abs(array3D[i][p3].p.z - array3D[i][p2].p.z ) >= params::curbHeight) &&\n abs(array3D[i][j].p.z - array3D[i][p3].p.z ) >= 0.05)\n {\n array3D[i][p2].isCurbPoint = 2;\n }\n }\n }\n }\n}" }, { "path": "src/z_zero_method.cpp", "content": "#include \"urban_road_filter/data_structures.hpp\"\n\nfloat params::angleFilter2; //angle between two vectors, while keeping Z = 0\n\nvoid Detector::zZeroMethod(std::vector>& array3D,int index,int* indexArray){\n /*-- step 1.: filtering the NON-road points --*/\n\n /*\n --> alpha - angle between the three points and the two vectors\n --> x1, x2, x3 - length of the sides of the triangle given by the three points\n --> curbPoints - estimated number of points on the roadside\n --> va1, va2, vb1, vb2 - the two vectors\n --> max1, max2 - not only angle, but height needs to be checked as well\n --> d - distance between the two outermost points - the rotation of the LiDAR and arc gaps make it necessary\n d - this variable also gets used later on in the code\n */\n float alpha, x1, x2, x3, va1, va2, vb1, vb2, max1, max2, d, bracket;\n for (int i = 0; i < index; i++){\n /*evaluation of points in an arc - z-zero method*/\n for (int j = params::curbPoints; j <= (indexArray[i] - 1) - params::curbPoints; j++)\n {\n d = sqrt(\n pow(array3D[i][j+params::curbPoints].p.x - array3D[i][j-params::curbPoints].p.x, 2) +\n pow(array3D[i][j+params::curbPoints].p.y - array3D[i][j-params::curbPoints].p.y, 2));\n\n /*set the distance to be less than 5 meters*/\n if (d < 5.0000)\n {\n /*initialization*/\n max1 = max2 = abs(array3D[i][j].p.z);\n va1 = va2 = vb1 = vb2 = 0;\n\n /*initializing vector 'a' and maximal height*/\n for (int k = j - 1; k >= j - params::curbPoints; k--)\n {\n va1 = va1 + (array3D[i][k].p.x - array3D[i][j].p.x);\n va2 = va2 + (array3D[i][k].p.y - array3D[i][j].p.y);\n if (abs(array3D[i][k].p.z) > max1)\n max1 = abs(array3D[i][k].p.z);\n }\n\n /*initializing vector 'b' and maximal height*/\n for (int k = j + 1; k <= j + params::curbPoints; k++)\n {\n vb1 = vb1 + (array3D[i][k].p.x - array3D[i][j].p.x );\n vb2 = vb2 + (array3D[i][k].p.y - array3D[i][j].p.y );\n if (abs(array3D[i][k].p.z ) > max2)\n max2 = abs(array3D[i][k].p.z);\n }\n\n va1 = (1 / (float)params::curbPoints) * va1;\n va2 = (1 / (float)params::curbPoints) * va2;\n vb1 = (1 / (float)params::curbPoints) * vb1;\n vb2 = (1 / (float)params::curbPoints) * vb2;\n\n bracket = (va1 * vb1 + va2 * vb2) / (sqrt(pow(va1, 2) + pow(va2, 2)) * sqrt(pow(vb1, 2) + pow(vb2, 2)));\n if (bracket < -1)\n bracket = -1;\n else if (bracket > 1)\n bracket = 1;\n\n alpha = acos(bracket) * 180 / M_PI;\n\n /*condition and assignment to group*/\n if (alpha <= params::angleFilter2 &&\n (max1 - abs(array3D[i][j].p.z ) >= params::curbHeight ||\n max2 - abs(array3D[i][j].p.z) >= params::curbHeight) &&\n abs(max1 - max2) >= 0.05)\n {\n array3D[i][j].isCurbPoint = 2;\n }\n }\n }\n }\n}" } ]