Simple LIDAR filter for self (vehicle) and outer area #2

README.md

@@ -11,9 +11,10 @@ The proposed system architecture is shown below. The following considerations ar
 
 All components are to be developed in ROS 2 Humble.
 
-``` mermaid
+```mermaid
 flowchart TD
-    S[State Machine <br>/plan_state_machine] -.->|/plan_state*| LS[LIDAR segementation<br>/prcp_ground_obstacle_segm_lidar]
+    S[State Machine <br>/plan_state_machine] -.->|/plan_state*| LP[LIDAR pre filter<br>/prcp_lidar_filtered]
+    LP --> LS[LIDAR segementation<br>/prcp_ground_obstacle_segm_lidar]
     S -.-> CS[Cone detection camera<br> and de-projection]
     S -.-> O[Object fusion]
     CS -->|/prcp_obj_list_camera| O
@@ -31,13 +32,12 @@ flowchart TD
     classDef white fill:#ffffff,stroke:#152742,stroke-width:2px,color:#15274
     classDef dash fill:#ffffff,stroke:#152742,stroke-width:2px,color:#15274, stroke-dasharray: 5 5
     classDef red fill:#ef4638,stroke:#152742,stroke-width:2px,color:#fff
-    class CS,LS,L,T,M,C white
+    class CS,LS,LP,L,T,M,C white
     class O light
     class S dash
     class CAN red
  ``` 
 
-![Architecture](img/arch.png)
 
 
 # Clone and build
@@ -57,15 +57,15 @@ git clone https://github.com/szenergy/formula_student_packages
 ## Build
 
 ``` bash
-cd ~/ros2_ws 
+sudo apt install ros-humble-pcl-ros
 ```
 
 ``` bash
-sudo apt install ros-humble-pcl-ros
+cd ~/ros2_ws 
 ```
 
 ``` bash
-colcon build --symlink-install --packages-select formula_student_bringup cone_detection_lidar
+colcon build --symlink-install --packages-select formula_student_bringup cone_detection_lidar cone_detection_camera lidar_pre_filter
 ```
 
 ## Run
@@ -83,6 +83,9 @@ ros2 launch cone_detection_lidar detection_simple.launch.py
 
 ## Directory structure
 
+> [!CAUTION]
+> If you move the packages to another directory, you need delete the build and install directories and rebuild the packages.
+
 ``` r
 ~/ros2_ws/src/
     ├── formula_student_packages # this repo

lidar_pre_filter/CMakeLists.txt

@@ -0,0 +1,62 @@
+cmake_minimum_required(VERSION 3.8)
+project(lidar_pre_filter)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(pcl_conversions REQUIRED) 
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+set(INCLUDE_DIRS 
+  include 
+  ${ament_cmake_INCLUDE_DIRS} 
+  ${rclcpp_INCLUDE_DIRS}
+  ${sensor_msgs_INCLUDE_DIRS}
+  ${pcl_ros_INCLUDE_DIRS} 
+  ${pcl_conversions_INCLUDE_DIRS}
+)
+include_directories(${INCLUDE_DIRS})
+
+set(ament_dependencies
+  rclcpp
+  sensor_msgs
+  pcl_conversions
+  PCL
+)
+
+add_executable(filter_vehicle src/filter_vehicle.cpp)
+target_link_libraries(filter_vehicle ${PCL_LIBRARIES})
+ament_target_dependencies(filter_vehicle  ${ament_dependencies} )
+target_compile_features(filter_vehicle PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+
+install(DIRECTORY 
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME})
+
+install(TARGETS 
+  filter_vehicle
+  DESTINATION lib/${PROJECT_NAME})
+
+
+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_package()

lidar_pre_filter/README.md

@@ -0,0 +1,30 @@
+# `lidar_pre_filter` ROS 2 package 
+
+[![Static Badge](https://img.shields.io/badge/ROS_2-Humble-34aec5)](https://docs.ros.org/en/humble/)
+
+## Parameters
+
+| Parameter    | Value e.g. | Description |
+|--------------|-------|-------------|
+| `cloud_in_topic`  | `/cloud` | Pointcloud to process |
+| `verbose1`  | `true` | More log info |
+| `verbose2`  | `true` | More log info |
+| `minX_over`  | -220.0| Outer crop box minX_over |
+| `maxX_over`  | 220.0 | Outer crop box maxX_over |
+| `minY_over`  | -220.0| Outer crop box minY_over |
+| `maxY_over`  | 220.0 | Outer crop box maxY_over |
+| `minZ_over`  | -10.0 | Outer crop box minZ_over |
+| `maxZ_over`  | -0.05 | Outer crop box maxZ_over |
+| `minX_vehicle` | -2.0 | Negative vehicle filter minX_vehicle |
+| `maxX_vehicle` | +2.0 | Negative vehicle filter maxX_vehicle |
+| `minY_vehicle` | -2.0 | Negative vehicle filter minY_vehicle |
+| `maxY_vehicle` | +2.0 | Negative vehicle filter maxY_vehicle |
+
+![Architecture](../img/filter01.png)
+
+
+## Run
+
+``` bash
+ros2 launch lidar_pre_filter filter_vehicle01.launch.py topic:=/lexus3/os_center/points
+```