Lidar fields validation 2

nav2_collision_monitor/README.md

@@ -103,3 +103,89 @@ The zones around the robot and the data sources are the same as for the Collisio
 Detailed configuration parameters, their description and how to setup a Collision Detector could be found at its [Configuration Guide](https://docs.nav2.org/configuration/packages/collision_monitor/configuring-collision-detector-node.html).
 
 The `CollisionMonitor` node makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).
+
+
+### Lidar estop prevention
+
+We use this with a safety lidar with e-stop zones depending on speed and steering angle. We want to avoid hitting such a zone.
+Thus, the collision monitor must ensure to limit the speed and steering angle so that the field chosen by
+the lidar is not intersecting with any lidar points (that would trigger an estop).
+
+#### Terminology
+
+* **Bucket**: a steering angle range (e.g. -12° to 12°). A bucket groups multiple fields that share the same angle range.
+* **Field**: a specific combination of steering angle bucket and velocity range. Each field corresponds to one `SubPolygonParameter` / velocity polygon entry.
+
+During field set creation, we also generate corresponding velocity polygons. They are about 50% larger than the corresponding
+e-stop zone. Example. Note that linear speed is for the steering wheel, not the baselink.
+
+```yaml
+forward_straight_mid_3:
+  points: # polygon here
+  linear_min: 0.5
+  linear_max: 0.7
+  steering_angle_min: -0.20944 # -12 degrees in radians
+  steering_angle_max: 0.20944 # 12 degrees in radians
+  linear_limit: 0.49 # steering wheel speed, not base link speed
+```
+
+Here, the **bucket** is the angle range [-12°, 12°]. The **field** is that bucket combined with the speed range [0.5, 0.7].
+
+So - if we are between 0.5 and 0.7 m/s, and hit that warning field, we must reduce our speed to *below* that field - because there is an obstacle in the polygon,
+which, if we approach it further, would trigger an estop.
+Once we are at 0.49, we will be in another field with a smaller polygon on the lidar side. Thus, the velocity polygon is also smaller. If we keep approaching the obstacle, we would slow down further.
+If the obstacle e.g. moves with us or is on the side, we can keep that speed.
+
+### Step 1: Normal collision monitor with velocity polygon
+
+### Step 2: Steering validation
+
+All speed comparisons against the low threshold use **steering wheel speed** (which accounts for the steering angle), except for the direction reversal check which uses baselink speed.
+
+We always assume that a matching field exists for the current velocity. If no field matches (e.g. the robot is outside all configured speed ranges), a warning is logged and steering validation is skipped for that cycle.
+
+If speed goes through zero (so sign(target speed) <> sign(current speed)):
+
+* if abs(current baselink speed) > low threshold → keep steering angle (we must anyway just slow down asap)
+* if abs(current baselink speed) < low threshold → allow steering
+
+else:
+
+1. check if both abs(target steering wheel speed) and abs(current steering wheel speed) are < low threshold. If yes → done
+2. check the one-step faster field in the **current** bucket for collision (same-bucket speed limit). Only the one-step faster field needs to be checked, even if target speed is in a much faster field. If the next field is collision-free → current bucket allows up to next field's max. If the next field has obstacles or no faster field exists → current bucket allows up to current field's max. This limit is always enforced — including when the target angle is in a different bucket — because the robot is still physically in the current bucket during any steering transition.
+3. check if target angle is in same bucket as current angle. If yes →
+   1. if abs(current steering wheel speed) < low threshold → done (robot is at standstill and must be free to start moving, the current field is not meaningful for speed limiting)
+   2. limit speed to the current bucket's speed limit from step 2. → done
+4. if target angle is in a different bucket → determine the direction of steering and find the neighbouring bucket from current angle in that direction. Only one bucket step at a time, even if the target angle is several buckets away.
+5. in the neighbouring bucket, determine max speed / valid field
+   1. start at fastest possible field (field for max(current speed, target speed)). If that is in collision, go down until a collision-free one is found. That one we call “valid” field. If all fields are in collision, use the slowest one with same speed sign in target direction (that is allowed even if in collision)
+6. now adapt speed and steering angle
+   1. limit target speed to the **minimum** of: max speed of valid field (from 5) and current bucket's speed limit (from 2)
+   2. if current speed is larger than max valid speed: limit steering angle to boundary of current bucket
+
+→ done
+
+After some iterations, the current speed will be in the valid field → AMR will be allowed to steer, as in 6b, the current speed will not be above max valid speed
+
+### Field exceedance prevention
+
+A fundamental safety invariant: **the commanded speed must never exceed the fastest configured field for the current steering angle bucket.** Violating this would cause the safety lidar to activate an e-stop, since no matching protective field exists for that speed/angle combination.
+
+#### Rule 1: Speed must stay within the available fields of the current bucket
+
+For any given steering angle, the lidar has a finite set of configured fields (each covering a speed range). The collision monitor must clamp the commanded speed so that it never exceeds the maximum speed of the fastest field defined for that bucket. If a bucket only has fields up to 0.5 m/s, the robot must not be commanded above 0.5 m/s while in that bucket — regardless of what the planner requests.
+
+This applies at all times, not only when obstacles are detected. Even in free space, sending a speed that has no corresponding field for the current angle would be unsafe.
+
+If no fields exist at all for the current physical steering angle, the velocity is set to zero.
+
+#### Rule 2: High speed only after entering the corresponding bucket
+
+When the robot transitions from a fully turned position (where only slow-speed fields exist) toward a straight position (where high-speed fields are available), the higher speed must **not** be sent until the steering angle has actually entered the bucket that supports that speed.
+
+Example: if the robot is fully turned left at 0.3 m/s and the planner requests straight driving at 1.0 m/s, the sequence must be:
+1. The robot begins straightening the steering while remaining at the slow speed allowed by the current (turned) bucket.
+2. Only once the steering angle crosses into the straight bucket — which has fields defined up to 1.0 m/s — may the speed be increased.
+3. Speed increases follow the normal one-field-step-at-a-time logic from Step 2.
+
+The opposite direction (straight → turned) is handled naturally: as the steering angle enters a more turned bucket with lower max speed, Rule 1 immediately clamps the speed down.

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

@@ -36,6 +36,7 @@
 #include "nav2_util/twist_publisher.hpp"
 #include "nav2_util/twist_subscriber.hpp"
 #include "nav2_msgs/msg/collision_monitor_state.hpp"
+#include "std_msgs/msg/float32.hpp"
 #include "nav2_msgs/srv/toggle.hpp"
 
 #include "nav2_collision_monitor/types.hpp"
@@ -267,8 +268,12 @@ class CollisionMonitor : public nav2::LifecycleNode
   rclcpp::Time stop_stamp_;
   /// @brief Timeout after which 0-velocity ceases to be published
   rclcpp::Duration stop_pub_timeout_;
+  /// @brief Whether steering validation is enabled
+  bool enable_steering_validation_;
   /// @brief Active polygons publisher
   rclcpp::Publisher<nav2_msgs::msg::ActiveVelocityPolygons>::SharedPtr active_polygons_pub_;
+  /// @brief Processing time publisher (ms)
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr processing_time_pub_;
 };  // class CollisionMonitor
 
 }  // namespace nav2_collision_monitor

nav2_collision_monitor/include/nav2_collision_monitor/source.hpp

@@ -158,6 +158,8 @@ class Source
   nav2::LifecycleNode::WeakPtr node_;
   /// @brief Collision monitor node logger stored for further usage
   rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")};
+  /// @brief Clock for throttled logging
+  rclcpp::Clock::SharedPtr clock_;
   /// @brief Dynamic parameters handler
   mutable std::mutex mutex_;
   rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_;

nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp

@@ -15,15 +15,18 @@
 #ifndef NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_
 #define NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_
 
+#include <algorithm>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include "geometry_msgs/msg/polygon_stamped.hpp"
 #include "nav2_collision_monitor/polygon.hpp"
 #include "nav2_collision_monitor/types.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "std_msgs/msg/string.hpp"
+#include "nav2_msgs/msg/steering_validation_debug.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tf2_ros/buffer.hpp"
 
@@ -74,6 +77,31 @@ class VelocityPolygon : public Polygon
    */
   void updatePolygon(const Velocity & cmd_vel_in) override;
 
+  /**
+   * @brief Validates steering to prevent triggering lidar e-stop zones.
+   * Implements Step 2 of the lidar e-stop prevention algorithm.
+   * @param cmd_vel_in Desired robot velocity (target)
+   * @param odom_vel Current robot velocity from odometry
+   * @param collision_points_map Map of source name to collision points
+   * @param robot_action Output robot action to modify if steering is restricted
+   * @return True if velocity was modified by steering validation
+   */
+  bool validateSteering(
+    const Velocity & cmd_vel_in,
+    const Velocity & odom_vel,
+    const std::unordered_map<std::string, std::vector<Point>> & collision_points_map,
+    Action & robot_action);
+
+  /**
+   * @brief Clamp commanded speed to the max available field at the current physical angle.
+   * Final safety gate to prevent field exceedance within the current bucket and
+   * during turned→straight transitions.
+   * @param odom_vel Current robot velocity from odometry (physical state)
+   * @param robot_action Robot action to modify if speed exceeds max field
+   * @return True if velocity was clamped
+   */
+  bool clampToMaxField(const Velocity & odom_vel, Action & robot_action);
+
 protected:
   /**
     * @brief Custom struct to store the parameters of the sub-polygon
@@ -119,8 +147,80 @@ class VelocityPolygon : public Polygon
    */
   bool isInRange(const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon_param);
 
+  /**
+   * @brief Compute steering angle from linear and angular velocity
+   * @param vel Velocity containing linear.x and angular.z
+   * @return Steering angle in radians
+   */
+  double computeSteeringAngle(const Velocity & vel) const;
+
+  /**
+   * @brief Convert baselink speed to steering wheel speed
+   * @param linear_vel Baselink linear velocity (twist.linear.x)
+   * @param angular_vel Baselink angular velocity (twist.angular.z)
+   * @return Steering wheel speed
+   */
+  double baselinkToSteeringSpeed(double linear_vel, double angular_vel) const;
+
+  /**
+   * @brief Convert steering wheel speed to baselink speed
+   * @param steering_speed Steering wheel speed
+   * @param steering_angle Steering angle in radians
+   * @return Baselink speed
+   */
+  double steeringToBaselinkSpeed(double steering_speed, double steering_angle) const;
+
+  /**
+   * @brief Convert steering angle back to angular velocity (twist.angular.z)
+   * @param baselink_speed Baselink linear speed
+   * @param steering_angle Steering angle in radians
+   * @return Angular velocity
+   */
+  double steeringAngleToTw(double baselink_speed, double steering_angle) const;
+
+  /**
+   * @brief Find the field (sub-polygon) matching a given steering wheel speed and steering angle
+   * @param steering_wheel_speed Speed in steering wheel frame
+   * @param steering_angle Steering angle in radians
+   * @return Pointer to matching sub-polygon, or nullptr if none found
+   */
+  const SubPolygonParameter * findField(
+    double steering_wheel_speed, double steering_angle) const;
+
+  /**
+   * @brief Find all fields (sub-polygons) matching a given steering angle and direction
+   * @param steering_angle Steering angle in radians
+   * @param forward If true, return only forward fields (linear_max >= 0);
+   *                if false, return only backward fields (linear_min <= 0)
+   * @return Vector of pointers to matching sub-polygons, sorted slowest (closest to zero) first
+   */
+  std::vector<const SubPolygonParameter *> findFieldsForAngle(
+    double steering_angle, bool forward) const;
+
+  /**
+   * @brief Check if a point is inside a given polygon (arbitrary vertices)
+   * @param point Point to check
+   * @param vertices Polygon vertices
+   * @return True if point is inside polygon
+   */
+  static bool isPointInsidePoly(const Point & point, const std::vector<Point> & vertices);
+
+  /**
+   * @brief Get number of collision points inside a specific sub-polygon
+   * @param sub_polygon Sub-polygon to check against
+   * @param collision_points_map Map of source name to collision points
+   * @return Number of collision points inside the sub-polygon
+   */
+  int getPointsInsideSubPolygon(
+    const SubPolygonParameter & sub_polygon,
+    const std::unordered_map<std::string, std::vector<Point>> & collision_points_map) const;
+
   // Clock
   rclcpp::Clock::SharedPtr clock_;
+  // Debug publisher for steering validation
+  rclcpp::Publisher<nav2_msgs::msg::SteeringValidationDebug>::SharedPtr steering_debug_pub_;
+  // Publisher for the next/valid field polygon being checked for obstacles
+  rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr next_field_poly_pub_;
   // Current subpolygon name
   std::string current_subpolygon_name_;
   // Variables
@@ -130,6 +230,8 @@ class VelocityPolygon : public Polygon
   double current_steering_angle_;
   /// @brief Distance between front and rear axes
   double wheelbase_;
+  /// @brief Speed below which steering is freely allowed
+  double low_speed_threshold_;
   /// @brief Vector to store the parameters of the sub-polygon
   std::vector<SubPolygonParameter> sub_polygons_;
 };  // class VelocityPolygon

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -14,6 +14,7 @@
 
 #include "nav2_collision_monitor/collision_monitor_node.hpp"
 
+#include <chrono>
 #include <exception>
 #include <utility>
 #include <functional>
@@ -81,6 +82,8 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state)
   cmd_vel_out_pub_ = std::make_unique<nav2_util::TwistPublisher>(node, cmd_vel_out_topic);
   active_polygons_pub_ = this->create_publisher<nav2_msgs::msg::ActiveVelocityPolygons>(
     "~/active_velocity_polygons");
+  processing_time_pub_ = this->create_publisher<std_msgs::msg::Float32>(
+    "~/processing_time_ms", rclcpp::QoS(1));
 
   if (!state_topic.empty()) {
     state_pub_ = this->create_publisher<nav2_msgs::msg::CollisionMonitorState>(
@@ -289,6 +292,8 @@ bool CollisionMonitor::getParameters(
   stop_pub_timeout_ = rclcpp::Duration::from_seconds(
     node->declare_or_get_parameter("stop_pub_timeout", 1.0));
 
+  enable_steering_validation_ = node->declare_or_get_parameter("enable_steering_validation", true);
+
   if (
     !configureSources(
       base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction))
@@ -427,6 +432,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
 {
   // Current timestamp for all inner routines prolongation
   rclcpp::Time curr_time = this->now();
+  const auto process_start = std::chrono::steady_clock::now();
 
   // Do nothing if main worker in non-active state
   if (!process_active_) {
@@ -451,6 +457,10 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
       if (!source->getData(curr_time, iter.first->second) &&
         source->getSourceTimeout().seconds() != 0.0)
       {
+        RCLCPP_WARN_THROTTLE(
+          get_logger(), *get_clock(), 2000,
+          "[%s]: Source getData() failed (no data / stale / TF error) — triggering STOP",
+          source->getSourceName().c_str());
         action_polygon = nullptr;
         robot_action.polygon_name = "invalid source";
         robot_action.action_type = STOP;
@@ -459,6 +469,12 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
         robot_action.req_vel.tw = 0.0;
         break;
       }
+      if (iter.first->second.empty()) {
+        RCLCPP_WARN_THROTTLE(
+          get_logger(), *get_clock(), 2000,
+          "[%s]: Source enabled and getData() succeeded but returned 0 points",
+          source->getSourceName().c_str());
+      }
     }
 
     if (collision_points_marker_pub_->get_subscription_count() > 0) {
@@ -499,6 +515,8 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
                        std::abs(last_odom_msg_.linear.y) < velocity_threshold &&
                        std::abs(last_odom_msg_.angular.z) < velocity_threshold;
 
+  std::shared_ptr<VelocityPolygon> active_limit_vel_polygon;
+
   for (std::shared_ptr<Polygon> polygon : polygons_) {
     if (!polygon->getEnabled() || !enabled_) {
       continue;
@@ -529,6 +547,14 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
       polygon->updatePolygon({last_odom_msg_.linear.x, last_odom_msg_.linear.y, last_odom_msg_.angular.z});
     }
 
+    // Track the active LIMIT VelocityPolygon (the one matching current speed/steering angle)
+    if (polygon->getActionType() == LIMIT) {
+      auto vp = std::dynamic_pointer_cast<VelocityPolygon>(polygon);
+      if (vp && vp->getCurrentSubPolygonName() != "none") {
+        active_limit_vel_polygon = vp;
+      }
+    }
+
     const ActionType at = polygon->getActionType();
     if (at == STOP || at == SLOWDOWN || at == LIMIT) {
       // Process STOP/SLOWDOWN for the selected polygon
@@ -545,6 +571,32 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
     }
   }
 
+  // Step 2: Steering validation (only for the active LIMIT VelocityPolygon)
+  if (enable_steering_validation_ && active_limit_vel_polygon &&
+    robot_action.action_type != STOP)
+  {
+    Velocity odom_vel{last_odom_msg_.linear.x, last_odom_msg_.linear.y, last_odom_msg_.angular.z};
+    if (active_limit_vel_polygon->validateSteering(
+        cmd_vel_in, odom_vel, sources_collision_points_map, robot_action))
+    {
+      action_polygon = active_limit_vel_polygon;
+    }
+  }
+
+  // Step 3: Field exceedance prevention — clamp speed to max available field
+  if (enable_steering_validation_) {
+    for (auto polygon : polygons_) {
+      auto vel_polygon = std::dynamic_pointer_cast<VelocityPolygon>(polygon);
+      if (!vel_polygon || !polygon->getEnabled()) {
+        continue;
+      }
+      Velocity odom_vel{last_odom_msg_.linear.x, last_odom_msg_.linear.y, last_odom_msg_.angular.z};
+      if (vel_polygon->clampToMaxField(odom_vel, robot_action)) {
+        action_polygon = polygon;
+      }
+    }
+  }
+
   if ((robot_action.polygon_name != robot_action_prev_.polygon_name) && enabled_) {
     // Report changed robot behavior
     notifyActionState(robot_action, action_polygon);
@@ -570,6 +622,12 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
   active_polygons_pub_->publish(std::move(msg));
 
   robot_action_prev_ = robot_action;
+
+  // Publish processing time
+  std_msgs::msg::Float32 time_msg;
+  time_msg.data = std::chrono::duration<float, std::milli>(
+    std::chrono::steady_clock::now() - process_start).count();
+  processing_time_pub_->publish(time_msg);
 }
 
 bool CollisionMonitor::processStopSlowdownLimit(

nav2_collision_monitor/src/costmap.cpp

@@ -70,6 +70,9 @@ bool CostmapSource::getData(
 {
   auto node = node_.lock();
   if (data_ == nullptr) {
+    RCLCPP_WARN_THROTTLE(
+      logger_, *clock_, 2000,
+      "[%s]: No costmap data received yet (data_ is null)", source_name_.c_str());
     return false;
   }