Added obstacle detection code

src/subsystems/navigation/obstacle_detection/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.8)
+project(obstacle_detection)
+
+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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# ---- Dependencies (Ament + ROS 2) ----
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_sensor_msgs REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(zed_msgs REQUIRED)
+
+# ---- PCL & Eigen ----
+find_package(PCL REQUIRED COMPONENTS common filters)
+
+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()

src/subsystems/navigation/obstacle_detection/cfg/solo_local_costmap.yaml

@@ -0,0 +1,51 @@
+# Minimal standalone local costmap focused on your obstacle stream
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      global_frame: odom
+      robot_base_frame: base_link
+      rolling_window: true
+      width: 10.0
+      height: 10.0
+      resolution: 0.05
+      update_frequency: 10.0
+      publish_frequency: 10.0
+      transform_tolerance: 0.1
+
+      # Simple rectangular footprint; adjust to your robot dims
+      footprint: [[0.35, 0.25], [0.35, -0.25], [-0.35, -0.25], [-0.35, 0.25]]
+      footprint_padding: 0.02
+
+      plugins: ["obstacle_layer", "inflation_layer"]
+
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: true
+        combination_method: 1             # MAX
+        observation_sources: obstacles, laser
+        obstacles:
+          data_type: PointCloud2
+          topic: /obstacles/points
+          marking: true
+          clearing: true
+          obstacle_range: 8.0
+          raytrace_range: 9.0
+          observation_persistence: 2.5    # seconds
+          expected_update_rate: 0.15      # seconds (6–10 Hz OK)
+
+        laser:
+          data_type: LaserScan
+          topic: /scan                    # TODO: Adjust based on actual topic name
+          marking: true
+          clearing: true
+          obstacle_range: 6.0             # TODO: Tune to the LiDAR (allegedly up to 30m but we might not need that)
+          raytrace_range: 7.0
+          observation_persistence: 2.5    # seconds
+
+        max_obstacle_height: 1.5          # global cap for layer
+
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        enabled: true
+        inflation_radius: 0.40
+        cost_scaling_factor: 3.0

src/subsystems/navigation/obstacle_detection/launch/solo_local_costmap.launch.py

@@ -0,0 +1,59 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+def generate_launch_description():
+    ns = LaunchConfiguration('namespace')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    nav_params = LaunchConfiguration('nav_params')
+    filter_params = LaunchConfiguration('filter_params')
+
+    return LaunchDescription([
+        DeclareLaunchArgument('namespace', default_value='', description='Robot namespace'),
+        DeclareLaunchArgument('use_sim_time', default_value='false'),
+        DeclareLaunchArgument(
+            'nav_params',
+            default_value=os.path.join(
+                get_package_share_directory('obstacle_detection'), 'config', 'solo_local_costmap.yaml')),
+        DeclareLaunchArgument(
+            'filter_params',
+            default_value=os.path.join(
+                get_package_share_directory('obstacle_detection'), 'cfg', 'obstacle_filter.yaml')),
+
+        # Obstacle filter node (subscribes ZED cloud, publishes /obstacles/points)
+        Node(
+            package='obstacle_detection',
+            executable='obstacle_filter_node',
+            namespace=ns,
+            name='obstacle_filter',
+            parameters=[filter_params, {'use_sim_time': use_sim_time}],
+            output='screen'
+        ),
+
+        # Standalone Nav2 costmap (no planners/controllers)
+        Node(
+            package='nav2_costmap_2d',
+            executable='nav2_costmap_2d',
+            namespace='local_costmap',          # keep topics under /local_costmap
+            name='local_costmap',
+            parameters=[nav_params, {'use_sim_time': use_sim_time}],
+            output='screen'
+        ),
+
+        # Lifecycle manager for just this one node
+        Node(
+            package='nav2_lifecycle_manager',
+            executable='lifecycle_manager',
+            namespace='local_costmap',
+            name='lifecycle_manager_local_costmap',
+            parameters=[{
+                'use_sim_time': use_sim_time,
+                'autostart': True,
+                'node_names': ['local_costmap']
+            }],
+            output='screen'
+        ),
+    ])

src/subsystems/navigation/obstacle_detection/package.xml

@@ -0,0 +1,41 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>obstacle_detection</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">abhinavkota06</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <!-- ROS 2 runtime/build deps -->
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_sensor_msgs</depend>
+  <depend>tf2_eigen</depend>
+  <depend>pcl_conversions</depend>
+  <depend>zed_interfaces</depend>
+  <depend>zed_msgs</depend>
+
+  <!-- System libs (via rosdep) -->
+  <build_depend>libpcl-dev</build_depend>
+  <exec_depend>libpcl-dev</exec_depend>
+
+  <!-- Eigen (portable find_package support) -->
+  <build_depend>eigen3_cmake_module</build_depend>
+  <build_export_depend>eigen3_cmake_module</build_export_depend>
+  <exec_depend>eigen3_cmake_module</exec_depend>
+  <build_depend>eigen</build_depend>
+  <exec_depend>eigen</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>