diff --git a/src/subsystems/navigation/obstacle_detection/CMakeLists.txt b/src/subsystems/navigation/obstacle_detection/CMakeLists.txt new file mode 100644 index 0000000..cc92e38 --- /dev/null +++ b/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() diff --git a/src/subsystems/navigation/obstacle_detection/cfg/solo_local_costmap.yaml b/src/subsystems/navigation/obstacle_detection/cfg/solo_local_costmap.yaml new file mode 100644 index 0000000..ed4679c --- /dev/null +++ b/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 diff --git a/src/subsystems/navigation/obstacle_detection/launch/solo_local_costmap.launch.py b/src/subsystems/navigation/obstacle_detection/launch/solo_local_costmap.launch.py new file mode 100644 index 0000000..b7cddd4 --- /dev/null +++ b/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' + ), + ]) diff --git a/src/subsystems/navigation/obstacle_detection/package.xml b/src/subsystems/navigation/obstacle_detection/package.xml new file mode 100644 index 0000000..8b292a1 --- /dev/null +++ b/src/subsystems/navigation/obstacle_detection/package.xml @@ -0,0 +1,41 @@ + + + + obstacle_detection + 0.0.0 + TODO: Package description + abhinavkota06 + TODO: License declaration + + ament_cmake + + + rclcpp + sensor_msgs + geometry_msgs + tf2 + tf2_ros + tf2_sensor_msgs + tf2_eigen + pcl_conversions + zed_interfaces + zed_msgs + + + libpcl-dev + libpcl-dev + + + eigen3_cmake_module + eigen3_cmake_module + eigen3_cmake_module + eigen + eigen + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/subsystems/navigation/obstacle_detection/src/obstacle_filter_node.cpp b/src/subsystems/navigation/obstacle_detection/src/obstacle_filter_node.cpp new file mode 100644 index 0000000..80d09d3 --- /dev/null +++ b/src/subsystems/navigation/obstacle_detection/src/obstacle_filter_node.cpp @@ -0,0 +1,212 @@ +#include +#include +#include + +#include +#include +#include + +#include +#include +#include // tf2::doTransform for PointCloud2 +#include // transformToEigen + +#include +#include +#include +#include + +#include +#include + +// ZED plane message (package: zed msgs) +#include + +// Run command below to publish camera -> base link transform +// ros2 run tf2_ros static_transform_publisher \ +// 0 0.30 0 \ +// -0.5 0.5 -0.5 -0.5 \ +// camera_link base_link + +using namespace std::chrono_literals; + +class ObstacleFilterNode : public rclcpp::Node { +public: + ObstacleFilterNode() + : Node("obstacle_filter"), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_){ + + input_topic_ = declare_parameter("input_topic", "/zed/zed_node/point_cloud/cloud_registered"); + output_topic_ = declare_parameter("output_topic", "/obstacles/points"); + + min_range_ = declare_parameter("min_range_m", 0.4); + max_range_ = declare_parameter("max_range_m", 6.0); + min_height_= declare_parameter("min_height_m", 0.05); + max_height_= declare_parameter("max_height_m", 1.00); + voxel_leaf_ = declare_parameter("voxel_leaf_m", 0.08); + + pub_ = create_publisher(output_topic_, rclcpp::SensorDataQoS{}); + sub_ = create_subscription( + input_topic_, rclcpp::SensorDataQoS(), + std::bind(&ObstacleFilterNode::cbCloud, this, std::placeholders::_1)); + + clicked_point_pub_ = create_publisher("/clicked_point", 10); + ground_plane_sub_ = create_subscription( + "/plane", rclcpp::QoS(10), + std::bind(&ObstacleFilterNode::onPlane, this, std::placeholders::_1)); + + // ---- 1 Hz timer publishes (0, 5, 5) in camera_link ---- + timer_ = create_wall_timer(1s, std::bind(&ObstacleFilterNode::onTimer, this)); + } + +private: + + struct PlaneCache { + rclcpp::Time stamp; + std::string frame_id; // frame of plane fields (from header.frame_id) + Eigen::Vector3f n_cam; // normal in plane frame (usually camera) + Eigen::Vector3f c_cam; // a point on plane in plane frame + bool valid{false}; + }; + + // 1 Hz: send the click ray point + void onTimer() { + geometry_msgs::msg::PointStamped pt; + pt.header.stamp = now(); + pt.header.frame_id = "camera_link"; // publish in camera frame + // Optical frame: X right, Y down, Z forward → “forward & down” = (0, +5, +5) + pt.point.x = 0.0; + pt.point.y = 5.0; + pt.point.z = 5.0; + clicked_point_pub_->publish(pt); + } + + void onPlane(const zed_msgs::msg::PlaneStamped::SharedPtr msg) { + std::lock_guard lock(plane_mtx_); + latest_plane_.stamp = msg->header.stamp; + latest_plane_.frame_id = msg->header.frame_id; + + // PlaneStamped.msg: normal & center are geometry_msgs/Point32 + latest_plane_.n_cam = Eigen::Vector3f( + msg->normal.x, msg->normal.y, msg->normal.z); + latest_plane_.c_cam = Eigen::Vector3f( + msg->center.x, msg->center.y, msg->center.z); + + // Normalize normal + float n = latest_plane_.n_cam.norm(); + if (n > 1e-6f) latest_plane_.n_cam /= n; + + latest_plane_.valid = true; + } + + void cbCloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + // Transform cloud → base_link + sensor_msgs::msg::PointCloud2 cloud_base; + try { + auto tf = tf_buffer_.lookupTransform( + "base_link", msg->header.frame_id, msg->header.stamp, 50ms); + tf2::doTransform(*msg, cloud_base, tf); + } catch (const tf2::TransformException &ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "TF (cloud) failed: %s", ex.what()); + return; + } + + // Convert to PCL + pcl::PointCloud::Ptr in(new pcl::PointCloud); + pcl::fromROSMsg(cloud_base, *in); + + // Downsample + pcl::PointCloud::Ptr ds(new pcl::PointCloud); + pcl::VoxelGrid voxel; + voxel.setInputCloud(in); + voxel.setLeafSize(static_cast(voxel_leaf_), static_cast(voxel_leaf_), static_cast(voxel_leaf_)); + voxel.filter(*ds); + + // Range gate + pcl::PointCloud::Ptr ranged(new pcl::PointCloud); + ranged->reserve(ds->size()); + const float min_r2 = static_cast(min_range_ * min_range_); + const float max_r2 = static_cast(max_range_ * max_range_); + for (const auto &p : ds->points) { + float r2 = p.x*p.x + p.y*p.y + p.z*p.z; + if (r2 >= min_r2 && r2 <= max_r2) ranged->push_back(p); + } + + // Transform latest plane → base_link, then filter by signed height + Eigen::Vector3f n_base(0.f, 0.f, 1.f); + float d_base = 0.f; + bool have_plane = false; + + { + std::lock_guard lock(plane_mtx_); + if (latest_plane_.valid) { + try { + auto tfp = tf_buffer_.lookupTransform( + "base_link", latest_plane_.frame_id, latest_plane_.stamp, 50ms); + Eigen::Isometry3d T = tf2::transformToEigen(tfp.transform); + Eigen::Matrix3f R = T.rotation().cast(); + Eigen::Vector3f t = T.translation().cast(); + + n_base = (R * latest_plane_.n_cam).normalized(); + Eigen::Vector3f c_base = R * latest_plane_.c_cam + t; + + // Orient normal upward if needed (helps consistency) + if (n_base.z() < 0.f) n_base = -n_base; + + d_base = -n_base.dot(c_base); + have_plane = true; + } catch (const tf2::TransformException &ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "TF (plane) failed: %s", ex.what()); + } + } + } + + pcl::PointCloud::Ptr band(new pcl::PointCloud); + band->reserve(ranged->size()); + + if (have_plane) { + const float hmin = static_cast(min_height_); + const float hmax = static_cast(max_height_); + for (const auto &p : ranged->points) { + float h = n_base.x()*p.x + n_base.y()*p.y + n_base.z()*p.z + d_base; + if (h >= hmin && h <= hmax) band->push_back(p); + } + } else { + // Fallback: simple z-band in base_link + pcl::PassThrough pass; + pass.setInputCloud(ranged); + pass.setFilterFieldName("z"); + pass.setFilterLimits(static_cast(min_height_), static_cast(max_height_)); + pass.filter(*band); + } + + // Publish filtered cloud (still in base_link) + sensor_msgs::msg::PointCloud2 out; + pcl::toROSMsg(*band, out); + out.header = cloud_base.header; // frame_id = base_link, stamp preserved + pub_->publish(out); + } + + rclcpp::Subscription::SharedPtr sub_; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr ground_plane_sub_; + rclcpp::Publisher::SharedPtr clicked_point_pub_; + rclcpp::TimerBase::SharedPtr timer_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + std::string input_topic_, output_topic_; + double min_range_, max_range_, min_height_, max_height_, voxel_leaf_; + + std::mutex plane_mtx_; + PlaneCache latest_plane_; +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}