Merge occupancy grid and obstacle tracking impl to main

.vscode/c_cpp_properties.json

@@ -0,0 +1,24 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/opt/ros/humble/include/**",
+        "/root/ws/src/ackermann_kf/go2_driver/include/**",
+        "/usr/include/**",
+        "${workspaceFolder}/**",
+        "/usr/include/opencv4",
+        "/root/ws/src/cev_msgs/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

CMakeLists.txt

@@ -10,46 +10,126 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(cev_msgs REQUIRED)
+find_package(PCL REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(pcl_ros REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(OpenCV REQUIRED)
+find_package(Open3D REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
-
+find_package(nav2_costmap_2d REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Obstacle.msg"
   "msg/ObstacleArray.msg"
-  DEPENDENCIES std_msgs geometry_msgs
+  "msg/Box.msg"
+  "msg/Boxes.msg"
+  DEPENDENCIES std_msgs geometry_msgs cev_msgs nav_msgs
 )
 
 add_executable(obstacle_node src/obstacle_node.cpp)
+add_executable(occupancy_grid src/occupancy_grid.cpp)
+add_executable(obstacle_tracker src/obstacle_tracker.cpp)
+add_executable(costmap_node src/costmap_node.cpp)
 
 ament_target_dependencies(
   obstacle_node 
   rclcpp 
+  cev_msgs
   sensor_msgs 
   std_msgs 
+  pcl_conversions
+  pcl_ros
+  geometry_msgs
+  tf2
+  tf2_geometry_msgs
+  visualization_msgs
+)
+
+ament_target_dependencies(
+  occupancy_grid
+  rclcpp 
+  cev_msgs
+  sensor_msgs 
+  std_msgs
+  nav_msgs
+  pcl_conversions
+  pcl_ros
+  geometry_msgs
+  tf2
+  tf2_geometry_msgs
+  visualization_msgs
+)
+
+ament_target_dependencies(
+  obstacle_tracker
+  rclcpp 
+  cev_msgs
+  sensor_msgs 
+  std_msgs
+  nav_msgs
+  pcl_conversions
+  pcl_ros
   geometry_msgs
   tf2
   tf2_geometry_msgs
   visualization_msgs
 )
 
+ament_target_dependencies(costmap_node
+  rclcpp
+  nav2_costmap_2d
+  sensor_msgs
+)
+
+set(CEV_ICP_INCLUDE lib/cev_icp/include)
+set(CEV_ICP_LIB lib/cev_icp/lib)
+
+include_directories(${CEV_ICP_INCLUDE})
+link_directories(${CEV_ICP_LIB})
+include_directories(
+  ${PCL_INCLUDE_DIRS}
+)
 
 target_link_libraries(obstacle_node
   ${OpenCV_LIBS}
+  ${PCL_LIBRARIES}
+  Open3D::Open3D
 )
 
-ament_export_dependencies(rosidl_default_runtime)
+target_link_libraries(occupancy_grid
+  ${OpenCV_LIBS}
+  ${PCL_LIBRARIES}
+)
 
+target_link_libraries(obstacle_tracker
+  ${OpenCV_LIBS}
+  ${PCL_LIBRARIES}
+  cevicp
+  Open3D::Open3D
+)
+
+ament_export_dependencies(rosidl_default_runtime)
 
 rosidl_target_interfaces(obstacle_node
   ${PROJECT_NAME} "rosidl_typesupport_cpp")
 
+rosidl_target_interfaces(occupancy_grid
+  ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+rosidl_target_interfaces(obstacle_tracker
+  ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
 install(TARGETS
   obstacle_node
+  occupancy_grid
+  obstacle_tracker
+  costmap_node
   DESTINATION lib/${PROJECT_NAME})
 
 

README.md

@@ -1 +1,21 @@
 # Obstacle_node
+Hi all, I'm going to start yapping again!
+
+## Angle Bins and Ray Tracing Via Bresenham's Line Algo
+
+![Occupancy Grid 1](demos/occ.png)
+![Occupancy Grid 2](demos/occupancygrid.png)
+
+## Multi-Hypothesis Tracking
+```
+cd Obstacle_node
+git submodule add https://github.com/cornellev/icp.git lib/cev_icp
+git submodule update --init --recursive
+cd lib/cev_icp
+sudo make install LIB_INSTALL=/usr/local/lib HEADER_INSTALL=/usr/local/include
+```
+
+# Important Links
+[Rasterize a point cloud](https://r-lidar.github.io/lasR/reference/rasterize.html)
+
+[Dynamic Obstacle Detection and Tracking Based on 3D Lidar](https://www.jstage.jst.go.jp/article/jaciii/22/5/22_602/_pdf)

config/costmap.yaml

@@ -0,0 +1,49 @@
+costmap_node:
+  ros__parameters:
+
+    # --- Frame config ---
+    global_frame: map
+    robot_base_frame: base_link
+    transform_tolerance: 0.5
+
+    # --- Update rates ---
+    update_frequency: 10.0
+    publish_frequency: 5.0
+
+    # --- Grid dimensions ---
+    rolling_window: false   # true = local costmap that follows robot
+    width: 20               # meters (only if rolling_window: false)
+    height: 20
+    resolution: 0.05        # meters/cell
+
+    # --- Robot shape ---
+    robot_radius: 0.3       # circular robot
+    # footprint: "[[0.3, 0.2], [0.3, -0.2], [-0.3, -0.2], [-0.3, 0.2]]"  # polygon
+
+    # --- Plugins ---
+    plugins: ["obstacle_layer", "inflation_layer"]
+
+    obstacle_layer:
+      plugin: "nav2_costmap_2d::ObstacleLayer"
+      enabled: true
+      observation_sources: pointcloud_sensor
+      pointcloud_sensor:
+        topic: /rslidar_points   # ← your PointCloud2 topic
+        data_type: PointCloud2
+        sensor_frame: ""                # leave blank to use cloud's frame_id
+        observation_persistence: 0.0   # seconds to keep obs (0 = latest only)
+        expected_update_rate: 0.0      # 0 = no timeout check
+        min_obstacle_height: 0.0       # ignore floor points
+        max_obstacle_height: 2.0       # ignore ceiling points
+        obstacle_max_range: 6.0        # max range to mark occupied
+        obstacle_min_range: 0.0
+        raytrace_max_range: 20.0        # max range to clear free space
+        raytrace_min_range: 0.0
+        marking: true                  # mark obstacles
+        clearing: true                 # raycast to clear free space
+
+    inflation_layer:
+      plugin: "nav2_costmap_2d::InflationLayer"
+      enabled: true
+      inflation_radius: 0.55           # meters, should be >= robot_radius
+      cost_scaling_factor: 10.0        # higher = sharper cost falloff

launch/launch_costmap.sh

@@ -0,0 +1,33 @@
+#!/bin/bash
+
+source /opt/ros/humble/setup.bash
+source /home/dev/ws/src/install/setup.bash
+
+echo "Building packages..."
+cd /home/dev/ws/src
+colcon build --packages-select cev_msgs obstacle
+
+echo "Sourcing workspace..."
+source /home/dev/ws/src/install/setup.bash
+
+echo "Starting TF publishers and costmap..."
+
+# Run each in background
+ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 map base_link &
+TF1_PID=$!
+
+ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link rslidar &
+TF2_PID=$!
+
+# Small delay to let TF come up before costmap starts
+sleep 1
+
+ros2 run obstacle costmap_node &
+COSTMAP_PID=$!
+
+echo "Running. PIDs: tf1=$TF1_PID tf2=$TF2_PID costmap=$COSTMAP_PID"
+echo "Press Ctrl+C to stop all."
+
+# Wait and kill all on Ctrl+C
+trap "kill $TF1_PID $TF2_PID $COSTMAP_PID; exit" SIGINT SIGTERM
+wait

msg/Box.msg

@@ -0,0 +1,23 @@
+# Box.msg
+#
+# float64 : cx
+# float64 : cy
+# float64 : vx
+# float64 : vy
+# float64 : length
+# float64 : width
+# float64 : yaw
+# float64 : yaw_rate
+# int32 : cid
+# bool : is_valid
+
+float64 cx
+float64 cy
+float64 vx
+float64 vy
+float64 length
+float64 width
+float64 yaw
+float64 yaw_rate
+int32 cid
+bool is_valid

msg/Boxes.msg

@@ -0,0 +1,7 @@
+# Boxes.msg
+#
+# Box[] : boxes
+
+# cev_msgs/Box[] boxes
+
+cev_msgs/Box[] obstacles