From e0aa66dff881832fd835d29b22b7fe582c18e771 Mon Sep 17 00:00:00 2001 From: cyy Date: Fri, 3 Jul 2026 08:17:16 +0000 Subject: [PATCH] LaserSafetyFilter: scan-based emergency brake + evasive turn before cmd publish --- CMakeLists.txt | 1 + include/car_nav_lite/laser_safety.hpp | 20 +++++++++++ src/laser_safety.cpp | 50 +++++++++++++++++++++++++++ src/main.cpp | 14 ++++++-- 4 files changed, 83 insertions(+), 2 deletions(-) create mode 100644 include/car_nav_lite/laser_safety.hpp create mode 100644 src/laser_safety.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 144ae3d..833c850 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,6 +27,7 @@ add_executable(nav_lite_node src/bt_executor.cpp src/session_mapper.cpp src/cone_updater.cpp + src/laser_safety.cpp ) target_include_directories(nav_lite_node PRIVATE diff --git a/include/car_nav_lite/laser_safety.hpp b/include/car_nav_lite/laser_safety.hpp new file mode 100644 index 0000000..d331af4 --- /dev/null +++ b/include/car_nav_lite/laser_safety.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include "car_nav_lite/types.hpp" + +namespace car_nav_lite { + +// Fast scan-based safety filter. Runs every frame before publishing cmd_vel. +// Checks if the planned command would hit anything in the latest scan. +struct LaserSafetyFilter { + bool enable = true; + double front_width = 0.30; // half-width of front safety zone (m) + double stop_dist = 0.35; // brake distance (m) + double slow_dist = 0.70; // slow-down distance (m) + double turn_wz = 0.8; // evasive turn rate (rad/s) + double max_range = 2.0; // max lookahead (m) + + Twist filter(const Twist& cmd, const LaserScan& scan) const; +}; + +} // namespace car_nav_lite diff --git a/src/laser_safety.cpp b/src/laser_safety.cpp new file mode 100644 index 0000000..a62dbf5 --- /dev/null +++ b/src/laser_safety.cpp @@ -0,0 +1,50 @@ +#include "car_nav_lite/laser_safety.hpp" +#include +#include + +namespace car_nav_lite { + +Twist LaserSafetyFilter::filter(const Twist& cmd, const LaserScan& scan) const { + Twist out = cmd; + if (!enable || scan.size() < 10) return out; + + double front_min = 999.0; + double left_sum = 0, right_sum = 0; + int left_n = 0, right_n = 0; + double half_w = front_width; + double stop = stop_dist + std::max(0.0, cmd.vx) * 0.5; + double slow = slow_dist + std::max(0.0, cmd.vx) * 0.5; + + for (int i = 0; i < scan.size(); ++i) { + double r = scan.ranges[i]; + if (r < 0.10 || r > max_range) continue; + double a = scan.angle_min + i * scan.angle_increment; + double x = r * std::cos(a); // body-frame + double y = r * std::sin(a); + if (x <= 0) continue; // behind car + + // Front safety zone + if (std::abs(y) < half_w && x < front_min) front_min = x; + + // Side clearance for evasive turn + if (x > 0 && x < 1.0) { + if (y > 0) { left_sum += r; left_n++; } + else { right_sum += r; right_n++; } + } + } + + double left_avg = left_n > 0 ? left_sum / left_n : 0; + double right_avg = right_n > 0 ? right_sum / right_n : 0; + + if (front_min < stop) { + // Emergency: stop and turn away + out.vx = 0.0; + out.wz = (left_avg > right_avg) ? turn_wz : -turn_wz; + } else if (front_min < slow) { + out.vx *= 0.3; + } + + return out; +} + +} // namespace car_nav_lite diff --git a/src/main.cpp b/src/main.cpp index 60008cc..5aeed60 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -22,6 +22,7 @@ #include "car_nav_lite/fence_model.hpp" #include "car_nav_lite/session_mapper.hpp" #include "car_nav_lite/cone_updater.hpp" +#include "car_nav_lite/laser_safety.hpp" using namespace std::chrono_literals; using namespace car_nav_lite; @@ -149,7 +150,9 @@ private: SessionMapper session_mapper_; FenceModel fence_model_; ConeUpdater cone_updater_; + LaserSafetyFilter safety_filter_; std::vector known_cones_; + rclcpp::Time last_cone_tick_{0,0,RCL_ROS_TIME}; enum SessionState { CALIBRATING, READY }; SessionState session_state_ = CALIBRATING; int calib_cnt_ = 0; @@ -261,9 +264,11 @@ private: pub_cmd_->publish(geometry_msgs::msg::Twist()); return; } - // READY: online cone discovery + optional dynamic obstacles + // READY: online cone discovery (3Hz) + optional dynamic obstacles if (has_scan_) { - if (online_cone_enable_) { + auto now = rclcpp::Clock().now(); + if (online_cone_enable_ && (now - last_cone_tick_).seconds() > 0.3) { + last_cone_tick_ = now; cone_updater_.update(latest_scan_, pose, fence_model_, known_cones_); auto new_cones = cone_updater_.popConfirmed(); for (auto& c : new_cones) { @@ -295,6 +300,11 @@ private: // Slow down if localization confidence is low if (localizer_.loc_conf_ < 0.3) { cmd.vx *= 0.4; cmd.wz *= 0.4; } + // Laser safety filter: last-moment scan check + if (has_scan_ && session_state_ == READY) { + Twist safe = safety_filter_.filter({(float)cmd.vx, (float)cmd.wz}, latest_scan_); + cmd.vx = safe.vx; cmd.wz = safe.wz; + } auto tw=geometry_msgs::msg::Twist();tw.linear.x=cmd.vx;tw.angular.z=cmd.wz; pub_cmd_->publish(tw); if(!bt_.currentPath().empty()){