LaserSafetyFilter: scan-based emergency brake + evasive turn before cmd publish

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

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

src/laser_safety.cpp

@@ -0,0 +1,50 @@
+#include "car_nav_lite/laser_safety.hpp"
+#include <cmath>
+#include <algorithm>
+
+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

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<Eigen::Vector2d> 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()){