From 2075a04fe2ba16164ee67d0c30a078fb61fccc59 Mon Sep 17 00:00:00 2001 From: Johannes Plapp Date: Tue, 10 Mar 2026 08:27:09 +0100 Subject: [PATCH 01/35] port lidar fields validation --- nav2_collision_monitor/README.md | 57 ++ .../collision_monitor_node.hpp | 2 + .../velocity_polygon.hpp | 87 +++ .../src/collision_monitor_node.cpp | 18 + .../src/velocity_polygon.cpp | 431 ++++++++++- .../test/collision_monitor_node_test.cpp | 37 - .../test/velocity_polygons_test.cpp | 684 ++++++++++++++++++ 7 files changed, 1258 insertions(+), 58 deletions(-) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index b8722a96f7c..043467bbf2e 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -103,3 +103,60 @@ 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 + +If speed goes through zero (so sign(target speed) <> sign(current speed)): + +* if abs(current speed) > low threshold → keep steering angle (we must anyway just slow down asap) +* if abs(current speed) < low threshold → allow steering + +else: + +1. check if both abs(target) and abs(current speed) are < low threshold. If yes → done +2. check if target angle is in same bucket as current angle. If yes → + 1. if the result velocity falls into the some faster field (same bucket), check the subsequent field for collision. Only the one-step faster field needs to be checked, even if target speed is in a much faster field. if in collision, limit speed to current field → then done + 2. 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. +3. 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) +4. now adapt speed and steering angle + 1. limit target speed to max speed of valid field + 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 4b, the current speed will not be above max valid speed \ No newline at end of file diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index b5ea3007b71..1daf3ee3b45 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -267,6 +267,8 @@ 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::SharedPtr active_polygons_pub_; }; // class CollisionMonitor diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp index afc855cdde3..02cb8b8a5e0 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp @@ -15,8 +15,10 @@ #ifndef NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_ #define NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_ +#include #include #include +#include #include #include "geometry_msgs/msg/polygon_stamped.hpp" @@ -74,6 +76,21 @@ 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> & collision_points_map, + Action & robot_action); + protected: /** * @brief Custom struct to store the parameters of the sub-polygon @@ -119,6 +136,74 @@ 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 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 & 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> & collision_points_map) const; + // Clock rclcpp::Clock::SharedPtr clock_; // Current subpolygon name @@ -130,6 +215,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 sub_polygons_; }; // class VelocityPolygon diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index c903333d36f..f9537aa93fc 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -289,6 +289,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)) @@ -545,6 +547,22 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: } } + // Step 2: Steering validation + if (enable_steering_validation_) { + for (auto polygon : polygons_) { + auto vel_polygon = std::dynamic_pointer_cast(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->validateSteering( + cmd_vel_in, odom_vel, sources_collision_points_map, 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); diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index d082fcfc53f..bdd68e7639f 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -14,6 +14,9 @@ #include "nav2_collision_monitor/velocity_polygon.hpp" +#include +#include + #include "nav2_ros_common/node_utils.hpp" namespace nav2_collision_monitor @@ -60,6 +63,9 @@ bool VelocityPolygon::getParameters( wheelbase_ = node->declare_or_get_parameter(polygon_name_ + ".wheelbase", 1.0); + low_speed_threshold_ = node->declare_or_get_parameter( + polygon_name_ + ".low_speed_threshold", 0.1); + for (std::string velocity_polygon_name : velocity_polygons) { // polygon points parameter std::vector poly; @@ -222,7 +228,13 @@ void VelocityPolygon::updatePolygon(const Velocity & cmd_vel_in) } slowdown_ratio_ = sub_polygon.slowdown_ratio_; - linear_limit_ = sub_polygon.linear_limit_; + if (sub_polygon.use_steering_angle_) { + // Convert linear_limit from steering wheel speed to baselink speed + linear_limit_ = steeringToBaselinkSpeed( + sub_polygon.linear_limit_, current_steering_angle_); + } else { + linear_limit_ = sub_polygon.linear_limit_; + } angular_limit_ = sub_polygon.angular_limit_; time_before_collision_ = sub_polygon.time_before_collision_; @@ -243,40 +255,50 @@ void VelocityPolygon::updatePolygon(const Velocity & cmd_vel_in) bool VelocityPolygon::isInRange( const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon) { - // 1. Check angular/steering range first - bool in_range; if (sub_polygon.use_steering_angle_) { - if (std::abs(cmd_vel_in.x) < 1e-6) { - current_steering_angle_ = (std::abs(cmd_vel_in.tw) < 1e-6) ? - 0.0 : - (cmd_vel_in.tw > 0 ? M_PI / 2 : -M_PI / 2); - } else { - double angular_vel = cmd_vel_in.tw; - if (cmd_vel_in.x < 0) { - angular_vel = -angular_vel; - } + current_steering_angle_ = computeSteeringAngle(cmd_vel_in); - current_steering_angle_ = std::atan2(wheelbase_ * angular_vel, std::abs(cmd_vel_in.x)); - } + // Convert baselink speed to steering wheel speed + double steering_wheel_speed = baselinkToSteeringSpeed(cmd_vel_in.x, cmd_vel_in.tw); RCLCPP_DEBUG( logger_, - "Calculated steering angle: %.2f (limits: %.2f to %.2f), linear_vel: %.2f, angular_vel: %.2f", + "Calculated steering angle: %.2f (limits: %.2f to %.2f), " + "baselink_vel: %.2f, steering_wheel_speed: %.2f, angular_vel: %.2f", current_steering_angle_, sub_polygon.steering_angle_min_, sub_polygon.steering_angle_max_, cmd_vel_in.x, + steering_wheel_speed, cmd_vel_in.tw ); - in_range = current_steering_angle_ <= sub_polygon.steering_angle_max_ && - current_steering_angle_ >= sub_polygon.steering_angle_min_; - } else { - in_range = - (cmd_vel_in.tw <= sub_polygon.theta_max_ && - cmd_vel_in.tw >= sub_polygon.theta_min_); + // Check linear range using steering wheel speed + bool in_range = steering_wheel_speed <= sub_polygon.linear_max_ && + steering_wheel_speed >= sub_polygon.linear_min_; + + if (!in_range) { + return false; + } + + // Check steering angle range + in_range &= current_steering_angle_ <= sub_polygon.steering_angle_max_ && + current_steering_angle_ >= sub_polygon.steering_angle_min_; + + return in_range; + } + + // Non-steering-angle mode: use baselink speed directly + bool in_range = cmd_vel_in.x <= sub_polygon.linear_max_ && + cmd_vel_in.x >= sub_polygon.linear_min_; + + if (!in_range) { + return false; } + in_range &= cmd_vel_in.tw <= sub_polygon.theta_max_ && + cmd_vel_in.tw >= sub_polygon.theta_min_; + if (holonomic_) { // 2. For holonomic robots: use speed magnitude + direction const double magnitude = std::hypot(cmd_vel_in.x, cmd_vel_in.y); @@ -307,4 +329,371 @@ bool VelocityPolygon::isInRange( return in_range; } +double VelocityPolygon::computeSteeringAngle(const Velocity & vel) const +{ + if (std::abs(vel.x) < 1e-6) { + return (std::abs(vel.tw) < 1e-6) ? 0.0 : (vel.tw > 0 ? M_PI / 2 : -M_PI / 2); + } + double angular_vel = vel.tw; + if (vel.x < 0) { + angular_vel = -angular_vel; + } + return std::atan2(wheelbase_ * angular_vel, std::abs(vel.x)); +} + +double VelocityPolygon::baselinkToSteeringSpeed( + double linear_vel, double angular_vel) const +{ + double magnitude = std::hypot(linear_vel, wheelbase_ * angular_vel); + if (linear_vel < 0.0) { + return -magnitude; + } + return magnitude; +} + +double VelocityPolygon::steeringToBaselinkSpeed( + double steering_speed, double steering_angle) const +{ + return steering_speed * std::cos(steering_angle); +} + +double VelocityPolygon::steeringAngleToTw( + double baselink_speed, double steering_angle) const +{ + // tw = tan(angle) * |v| / wheelbase, sign-corrected for reverse + double tw = std::tan(steering_angle) * std::abs(baselink_speed) / wheelbase_; + if (baselink_speed < 0.0) { + tw = -tw; + } + return tw; +} + +const VelocityPolygon::SubPolygonParameter * VelocityPolygon::findField( + double steering_wheel_speed, double steering_angle) const +{ + for (const auto & sp : sub_polygons_) { + if (!sp.use_steering_angle_) { + continue; + } + if (steering_wheel_speed >= sp.linear_min_ && steering_wheel_speed <= sp.linear_max_ && + steering_angle >= sp.steering_angle_min_ && steering_angle <= sp.steering_angle_max_) + { + return &sp; + } + } + return nullptr; +} + +std::vector +VelocityPolygon::findFieldsForAngle(double steering_angle, bool forward) const +{ + std::vector result; + for (const auto & sp : sub_polygons_) { + if (!sp.use_steering_angle_) { + continue; + } + if (steering_angle >= sp.steering_angle_min_ && steering_angle <= sp.steering_angle_max_) { + if (forward ? (sp.linear_max_ >= 0) : (sp.linear_min_ <= 0)) { + result.push_back(&sp); + } + } + } + // Sort by speed magnitude ascending (slowest/closest to zero first) + std::sort(result.begin(), result.end(), + [](const SubPolygonParameter * a, const SubPolygonParameter * b) { + return std::abs(a->linear_min_) < std::abs(b->linear_min_); + }); + return result; +} + +bool VelocityPolygon::isPointInsidePoly( + const Point & point, const std::vector & vertices) +{ + // Ray-casting algorithm (same as Polygon::isPointInside but for arbitrary vertices) + const int poly_size = static_cast(vertices.size()); + int i, j; + bool res = false; + + i = poly_size - 1; + for (j = 0; j < poly_size; j++) { + if ((point.y <= vertices[i].y) == (point.y > vertices[j].y)) { + const double x_inter = vertices[i].x + + (point.y - vertices[i].y) * (vertices[j].x - vertices[i].x) / + (vertices[j].y - vertices[i].y); + if (x_inter > point.x) { + res = !res; + } + } + i = j; + } + return res; +} + +int VelocityPolygon::getPointsInsideSubPolygon( + const SubPolygonParameter & sub_polygon, + const std::unordered_map> & collision_points_map) const +{ + int num = 0; + std::vector polygon_sources_names = getSourcesNames(); + + for (const auto & source_name : polygon_sources_names) { + const auto & iter = collision_points_map.find(source_name); + if (iter != collision_points_map.end()) { + for (const auto & point : iter->second) { + if (isPointInsidePoly(point, sub_polygon.poly_)) { + num++; + } + } + } + } + + return num; +} + +bool VelocityPolygon::validateSteering( + const Velocity & cmd_vel_in, + const Velocity & odom_vel, + const std::unordered_map> & collision_points_map, + Action & robot_action) +{ + // Only applies to steering-angle-based velocity polygons + if (sub_polygons_.empty() || !sub_polygons_[0].use_steering_angle_) { + return false; + } + + const double target_speed = cmd_vel_in.x; + const double current_speed = odom_vel.x; + + const double target_steering_angle = computeSteeringAngle(cmd_vel_in); + const double current_sa = computeSteeringAngle(odom_vel); + + RCLCPP_INFO( + logger_, + "[%s] validateSteering: target_speed=%.3f, current_speed=%.3f, " + "target_sa=%.3f, current_sa=%.3f, cmd_vel_in=(%.3f, %.3f, %.3f), odom_vel=(%.3f, %.3f, %.3f)", + polygon_name_.c_str(), target_speed, current_speed, + target_steering_angle, current_sa, + cmd_vel_in.x, cmd_vel_in.y, cmd_vel_in.tw, + odom_vel.x, odom_vel.y, odom_vel.tw); + + bool modified = false; + Velocity result_vel = robot_action.req_vel; + + // Check if speed crosses zero (direction reversal) + bool crosses_zero = (target_speed > 0 && current_speed < 0) || + (target_speed < 0 && current_speed > 0); + + if (crosses_zero) { + if (std::abs(current_speed) > low_speed_threshold_) { + // Must decelerate first — clamp tw to maintain current steering angle + result_vel.tw = steeringAngleToTw(result_vel.x, current_sa); + modified = true; + RCLCPP_INFO( + logger_, + "[%s] validateSteering: direction reversal detected, clamping tw to maintain " + "current_sa=%.3f (current_speed=%.3f > threshold=%.3f)", + polygon_name_.c_str(), current_sa, current_speed, low_speed_threshold_); + } + // else: abs(current) < threshold → allow steering freely + if (modified) { + robot_action.req_vel = result_vel; + robot_action.polygon_name = polygon_name_; + robot_action.action_type = LIMIT; + } + return modified; + } + + // Speed does NOT cross zero + // 1. Both abs(target) and abs(current) below threshold → done + if (std::abs(target_speed) < low_speed_threshold_ && + std::abs(current_speed) < low_speed_threshold_) + { + return false; + } + + double target_sw_speed = baselinkToSteeringSpeed(cmd_vel_in.x, cmd_vel_in.tw); + double current_sw_speed = baselinkToSteeringSpeed(odom_vel.x, odom_vel.tw); + + const SubPolygonParameter * current_field = findField(current_sw_speed, current_sa); + + RCLCPP_INFO( + logger_, + "[%s] validateSteering: target_sw_speed=%.3f, current_sw_speed=%.3f, " + "current_field=%s", + polygon_name_.c_str(), target_sw_speed, current_sw_speed, + current_field ? current_field->velocity_polygon_name_.c_str() : "null"); + + // 2. Check if target angle is in same bucket (angle range) as current + bool same_bucket = current_field != nullptr && + target_steering_angle >= current_field->steering_angle_min_ && + target_steering_angle <= current_field->steering_angle_max_; + + RCLCPP_INFO( + logger_, + "[%s] validateSteering: same_bucket=%s", + polygon_name_.c_str(), same_bucket ? "true" : "false"); + + if (same_bucket) { + // If result velocity falls into some faster field (same bucket), check the + // one-step-faster field for collision. If in collision, limit to current field. + double result_sw_speed = baselinkToSteeringSpeed(result_vel.x, result_vel.tw); + const SubPolygonParameter * result_field = findField(result_sw_speed, target_steering_angle); + if (result_field != nullptr && result_field != current_field && + std::abs(result_sw_speed) > std::abs(current_sw_speed)) + { + bool forward = current_sw_speed >= 0; + auto fields_at_angle = findFieldsForAngle(target_steering_angle, forward); + for (size_t i = 0; i < fields_at_angle.size(); i++) { + if (fields_at_angle[i] == current_field && i + 1 < fields_at_angle.size()) { + const SubPolygonParameter * next_field = fields_at_angle[i + 1]; + if (getPointsInsideSubPolygon(*next_field, collision_points_map) >= min_points_) { + // Forward: limit to linear_max (upper bound) + // Backward: limit to linear_min (lower bound, more negative = faster) + double limit_sw = (current_sw_speed >= 0) ? + current_field->linear_max_ : current_field->linear_min_; + double max_baselink = steeringToBaselinkSpeed(limit_sw, current_sa); + if (std::abs(result_vel.x) > std::abs(max_baselink)) { + RCLCPP_INFO( + logger_, + "[%s] validateSteering: same_bucket speed limit — next field '%s' in collision, " + "limiting vel.x from %.3f to %.3f (sw_limit=%.3f)", + polygon_name_.c_str(), next_field->velocity_polygon_name_.c_str(), + result_vel.x, max_baselink, limit_sw); + result_vel.x = max_baselink; + result_vel.tw = steeringAngleToTw(result_vel.x, target_steering_angle); + modified = true; + } + } + break; + } + } + } + // Same bucket → done + if (modified) { + robot_action.req_vel = result_vel; + robot_action.polygon_name = polygon_name_; + robot_action.action_type = LIMIT; + } + return modified; + } + + // 3. Different bucket — find neighbouring bucket (one step in steering direction) + RCLCPP_INFO( + logger_, + "[%s] validateSteering: different bucket — target_sa=%.3f outside current field [%.3f, %.3f]", + polygon_name_.c_str(), target_steering_angle, + current_field->steering_angle_min_, current_field->steering_angle_max_); + double neighbour_angle; + if (target_steering_angle > current_sa) { + neighbour_angle = current_field->steering_angle_max_; + } else { + neighbour_angle = current_field->steering_angle_min_; + } + // Step just past the boundary to land in the neighbouring bucket + constexpr double kAngleEps = 0.01; + double lookup_angle = (target_steering_angle > current_sa) ? + neighbour_angle + kAngleEps : neighbour_angle - kAngleEps; + + bool forward = target_sw_speed >= 0; + auto neighbour_fields = findFieldsForAngle(lookup_angle, forward); + if (neighbour_fields.empty()) { + return false; + } + + // Find the fastest field that covers max(|current|, |target|) speed + double max_sw_speed = std::max(std::abs(current_sw_speed), std::abs(target_sw_speed)); + + // Find the valid field: start from fastest covering field, walk down + const SubPolygonParameter * valid_field = nullptr; + int start_idx = static_cast(neighbour_fields.size()) - 1; + + // Find the starting field (fastest that covers max_sw_speed) + for (int i = start_idx; i >= 0; i--) { + if (max_sw_speed >= std::abs(neighbour_fields[i]->linear_min_) && + max_sw_speed <= std::abs(neighbour_fields[i]->linear_max_)) + { + start_idx = i; + break; + } + // If max_sw_speed is beyond all fields, start from the fastest + if (i == 0) { + start_idx = static_cast(neighbour_fields.size()) - 1; + } + } + + // Check if fastest field is collision-free → done + if (getPointsInsideSubPolygon(*neighbour_fields[start_idx], collision_points_map) < min_points_) { + return false; + } + + // Walk down speed fields until a collision-free one is found + for (int i = start_idx; i >= 0; i--) { + if (getPointsInsideSubPolygon(*neighbour_fields[i], collision_points_map) < min_points_) { + valid_field = neighbour_fields[i]; + break; + } + } + + if (valid_field == nullptr) { + // All fields in collision — use the slowest field in the target direction + // (allowed even if in collision) + valid_field = neighbour_fields[0]; // sorted ascending, index 0 is slowest + RCLCPP_INFO( + logger_, + "[%s] validateSteering: all neighbour fields in collision, " + "falling back to slowest field '%s'", + polygon_name_.c_str(), valid_field->velocity_polygon_name_.c_str()); + } else { + RCLCPP_INFO( + logger_, + "[%s] validateSteering: valid neighbour field found: '%s'", + polygon_name_.c_str(), valid_field->velocity_polygon_name_.c_str()); + } + + // 4. Adapt speed and steering angle + // 4a. Limit target speed to valid field's speed boundary (converted to baselink) + // Forward: limit to linear_max; Backward: limit to linear_min + double valid_limit_sw = (target_sw_speed >= 0) ? + valid_field->linear_max_ : valid_field->linear_min_; + double valid_max_baselink = steeringToBaselinkSpeed( + valid_limit_sw, neighbour_angle); + if (std::abs(result_vel.x) > std::abs(valid_max_baselink)) { + RCLCPP_INFO( + logger_, + "[%s] validateSteering: limiting speed from %.3f to %.3f " + "(valid_limit_sw=%.3f, neighbour_angle=%.3f)", + polygon_name_.c_str(), result_vel.x, valid_max_baselink, + valid_limit_sw, neighbour_angle); + result_vel.x = valid_max_baselink; + } + + // 4b. Only limit steering angle if current speed is larger than max valid speed + double current_baselink_abs = std::abs(current_speed); + double valid_max_baselink_abs = std::abs(valid_max_baselink); + if (current_baselink_abs > valid_max_baselink_abs && current_field != nullptr) { + double limited_sa; + if (target_steering_angle > current_sa) { + limited_sa = current_field->steering_angle_max_; + } else { + limited_sa = current_field->steering_angle_min_; + } + RCLCPP_INFO( + logger_, + "[%s] validateSteering: limiting steering angle to %.3f " + "(current_speed=%.3f > valid_max=%.3f)", + polygon_name_.c_str(), limited_sa, current_baselink_abs, valid_max_baselink_abs); + result_vel.tw = steeringAngleToTw(result_vel.x, limited_sa); + } + + RCLCPP_INFO( + logger_, + "[%s] validateSteering: final result_vel=(%.3f, %.3f, %.3f)", + polygon_name_.c_str(), result_vel.x, result_vel.y, result_vel.tw); + + robot_action.req_vel = result_vel; + robot_action.polygon_name = polygon_name_; + robot_action.action_type = LIMIT; + return true; +} + } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index d18228d31e4..b4042e310e7 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -429,43 +429,6 @@ void Tester::addPolygonVelocitySubPolygon( polygon_name + "." + sub_polygon_name + ".theta_max", rclcpp::ParameterValue(theta_max)); } -void Tester::addPolygonVelocitySubPolygon( - const std::string & polygon_name, const std::string & sub_polygon_name, - const double linear_min, const double linear_max, - const double theta_min, const double theta_max, - const double size) -{ - const std::string points = "[[" + - std::to_string(size) + ", " + std::to_string(size) + "], [" + - std::to_string(size) + ", " + std::to_string(-size) + "], [" + - std::to_string(-size) + ", " + std::to_string(-size) + "], [" + - std::to_string(-size) + ", " + std::to_string(size) + "]]"; - cm_->declare_parameter( - polygon_name + "." + sub_polygon_name + ".points", rclcpp::ParameterValue(points)); - cm_->set_parameter( - rclcpp::Parameter(polygon_name + "." + sub_polygon_name + ".points", points)); - - cm_->declare_parameter( - polygon_name + "." + sub_polygon_name + ".linear_min", rclcpp::ParameterValue(linear_min)); - cm_->set_parameter( - rclcpp::Parameter(polygon_name + "." + sub_polygon_name + ".linear_min", linear_min)); - - cm_->declare_parameter( - polygon_name + "." + sub_polygon_name + ".linear_max", rclcpp::ParameterValue(linear_max)); - cm_->set_parameter( - rclcpp::Parameter(polygon_name + "." + sub_polygon_name + ".linear_max", linear_max)); - - cm_->declare_parameter( - polygon_name + "." + sub_polygon_name + ".theta_min", rclcpp::ParameterValue(theta_min)); - cm_->set_parameter( - rclcpp::Parameter(polygon_name + "." + sub_polygon_name + ".theta_min", theta_min)); - - cm_->declare_parameter( - polygon_name + "." + sub_polygon_name + ".theta_max", rclcpp::ParameterValue(theta_max)); - cm_->set_parameter( - rclcpp::Parameter(polygon_name + "." + sub_polygon_name + ".theta_max", theta_max)); -} - void Tester::addSource( const std::string & source_name, const SourceType type) { diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index 9c70326e360..2b7a477170a 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -130,6 +131,58 @@ class VelocityPolygonWrapper : public nav2_collision_monitor::VelocityPolygon { return sub_polygons_; } + + double getCurrentSteeringAngle() const + { + return current_steering_angle_; + } + + double callComputeSteeringAngle(const nav2_collision_monitor::Velocity & vel) const + { + return computeSteeringAngle(vel); + } + + double callBaselinkToSteeringSpeed(double linear_vel, double angular_vel) const + { + return baselinkToSteeringSpeed(linear_vel, angular_vel); + } + + double callSteeringToBaselinkSpeed(double steering_speed, double steering_angle) const + { + return steeringToBaselinkSpeed(steering_speed, steering_angle); + } + + double callSteeringAngleToTw(double baselink_speed, double steering_angle) const + { + return steeringAngleToTw(baselink_speed, steering_angle); + } + + const SubPolygonParameter * callFindField( + double steering_wheel_speed, double steering_angle) const + { + return findField(steering_wheel_speed, steering_angle); + } + + std::vector callFindFieldsForAngle( + double steering_angle, bool forward) const + { + return findFieldsForAngle(steering_angle, forward); + } + + static bool callIsPointInsidePoly( + const nav2_collision_monitor::Point & point, + const std::vector & vertices) + { + return isPointInsidePoly(point, vertices); + } + + int callGetPointsInsideSubPolygon( + const SubPolygonParameter & sub_polygon, + const std::unordered_map> & collision_points_map) const + { + return getPointsInsideSubPolygon(sub_polygon, collision_points_map); + } }; // VelocityPolygonWrapper class Tester : public ::testing::Test @@ -153,8 +206,18 @@ class Tester : public ::testing::Test const double direction_end_angle, const double direction_start_angle, const std::string & polygon_points, const bool is_holonomic); + void addSteeringAngleSubPolygon( + const std::string & sub_polygon_name, + const double linear_min, const double linear_max, + const double steering_angle_min, const double steering_angle_max, + const std::string & polygon_points); + void setSteeringVelocityPolygonParameters( + const double wheelbase, const double low_speed_threshold, + const std::vector & sub_polygon_names); + // Creating routines void createVelocityPolygon(const std::string & action_type, const bool is_holonomic); + void createSteeringVelocityPolygon(const std::string & action_type); // Wait until polygon will be received bool waitPolygon( @@ -333,6 +396,79 @@ void Tester::addPolygonVelocitySubPolygon( } } +void Tester::addSteeringAngleSubPolygon( + const std::string & sub_polygon_name, + const double linear_min, const double linear_max, + const double steering_angle_min, const double steering_angle_max, + const std::string & polygon_points) +{ + const std::string prefix = std::string(POLYGON_NAME) + "." + sub_polygon_name; + + test_node_->declare_parameter( + prefix + ".points", rclcpp::ParameterValue(polygon_points)); + test_node_->set_parameter( + rclcpp::Parameter(prefix + ".points", polygon_points)); + + test_node_->declare_parameter( + prefix + ".linear_min", rclcpp::ParameterValue(linear_min)); + test_node_->set_parameter( + rclcpp::Parameter(prefix + ".linear_min", linear_min)); + + test_node_->declare_parameter( + prefix + ".linear_max", rclcpp::ParameterValue(linear_max)); + test_node_->set_parameter( + rclcpp::Parameter(prefix + ".linear_max", linear_max)); + + test_node_->declare_parameter( + prefix + ".steering_angle_min", rclcpp::ParameterValue(steering_angle_min)); + test_node_->set_parameter( + rclcpp::Parameter(prefix + ".steering_angle_min", steering_angle_min)); + + test_node_->declare_parameter( + prefix + ".steering_angle_max", rclcpp::ParameterValue(steering_angle_max)); + test_node_->set_parameter( + rclcpp::Parameter(prefix + ".steering_angle_max", steering_angle_max)); +} + +void Tester::setSteeringVelocityPolygonParameters( + const double wheelbase, const double low_speed_threshold, + const std::vector & sub_polygon_names) +{ + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".holonomic", rclcpp::ParameterValue(false)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".holonomic", false)); + + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".wheelbase", rclcpp::ParameterValue(wheelbase)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".wheelbase", wheelbase)); + + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".low_speed_threshold", + rclcpp::ParameterValue(low_speed_threshold)); + test_node_->set_parameter( + rclcpp::Parameter( + std::string(POLYGON_NAME) + ".low_speed_threshold", low_speed_threshold)); + + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".velocity_polygons", + rclcpp::ParameterValue(sub_polygon_names)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".velocity_polygons", sub_polygon_names)); +} + +void Tester::createSteeringVelocityPolygon(const std::string & action_type) +{ + setCommonParameters(POLYGON_NAME, action_type); + + velocity_polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(velocity_polygon_->configure()); + velocity_polygon_->activate(); +} + void Tester::createVelocityPolygon(const std::string & action_type, const bool is_holonomic) { setCommonParameters(POLYGON_NAME, action_type); @@ -547,6 +683,554 @@ TEST_F(Tester, testVelocityPolygonHolonomicVelocitySwitching) } +// ==================== Steering wheel speed conversion tests ==================== + +// Polygon for steering tests: a simple square around the robot +static const char STEERING_POLYGON_STR[]{ + "[[1.0, 0.5], [1.0, -0.5], [-0.5, -0.5], [-0.5, 0.5]]"}; +// Smaller polygon for the slower field +static const char STEERING_POLYGON_SLOW_STR[]{ + "[[0.5, 0.3], [0.5, -0.3], [-0.3, -0.3], [-0.3, 0.3]]"}; +// Larger polygon for the faster field +static const char STEERING_POLYGON_FAST_STR[]{ + "[[1.5, 0.8], [1.5, -0.8], [-0.8, -0.8], [-0.8, 0.8]]"}; + +static const double WHEELBASE{1.0}; +static const double LOW_SPEED_THRESHOLD{0.1}; + +TEST_F(Tester, testBaselinkToSteeringSpeedConversion) +{ + // Setup: create a simple steering velocity polygon + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"slow", "fast"}); + addSteeringAngleSubPolygon("slow", 0.0, 0.5, -0.5, 0.5, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("fast", 0.5, 1.0, -0.5, 0.5, STEERING_POLYGON_FAST_STR); + createSteeringVelocityPolygon("limit"); + + // At zero angular vel, steering speed == baselink speed + EXPECT_NEAR(velocity_polygon_->callBaselinkToSteeringSpeed(1.0, 0.0), 1.0, 1e-6); + EXPECT_NEAR(velocity_polygon_->callBaselinkToSteeringSpeed(-0.5, 0.0), -0.5, 1e-6); + + // At 60 deg steering: tw = vx * tan(60°) / wheelbase = sqrt(3), expected = hypot(1, sqrt(3)) = 2 + EXPECT_NEAR(velocity_polygon_->callBaselinkToSteeringSpeed(1.0, std::sqrt(3.0)), 2.0, 1e-6); + + // At 45 deg steering: tw = vx * tan(45°) / wheelbase = 1.0, expected = hypot(1, 1) = sqrt(2) + double expected = std::sqrt(2.0); + EXPECT_NEAR(velocity_polygon_->callBaselinkToSteeringSpeed(1.0, 1.0), expected, 1e-6); + + // At 90 deg steering (vx=0, tw!=0): continuous result, not infinity + double result = velocity_polygon_->callBaselinkToSteeringSpeed(0.0, 1.0); + EXPECT_NEAR(result, WHEELBASE * 1.0, 1e-6); + + // Negative speed: sign is preserved + EXPECT_NEAR(velocity_polygon_->callBaselinkToSteeringSpeed(-1.0, 1.0), -expected, 1e-6); +} + +TEST_F(Tester, testSteeringToBaselinkSpeedConversion) +{ + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"slow"}); + addSteeringAngleSubPolygon("slow", 0.0, 0.5, -0.5, 0.5, STEERING_POLYGON_SLOW_STR); + createSteeringVelocityPolygon("limit"); + + // At zero steering angle, baselink speed == steering speed + EXPECT_NEAR(velocity_polygon_->callSteeringToBaselinkSpeed(1.0, 0.0), 1.0, 1e-6); + + // At 60 degrees, cos(60°)=0.5, so baselink = steering * 0.5 + EXPECT_NEAR(velocity_polygon_->callSteeringToBaselinkSpeed(1.0, M_PI / 3), 0.5, 1e-6); + + // Roundtrip: baselink -> steering -> baselink should be identity + double sa = 0.3; // steering angle + double baselink = 0.8; + // tw = |vx| * tan(sa) / wheelbase (for vx > 0, wheelbase = 1.0) + double tw = baselink * std::tan(sa) / WHEELBASE; + double steering = velocity_polygon_->callBaselinkToSteeringSpeed(baselink, tw); + double roundtrip = velocity_polygon_->callSteeringToBaselinkSpeed(steering, sa); + EXPECT_NEAR(roundtrip, baselink, 1e-6); +} + +TEST_F(Tester, testSteeringAngleToTw) +{ + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"slow"}); + addSteeringAngleSubPolygon("slow", 0.0, 0.5, -0.5, 0.5, STEERING_POLYGON_SLOW_STR); + createSteeringVelocityPolygon("limit"); + + // At zero steering angle, tw should be 0 + EXPECT_NEAR(velocity_polygon_->callSteeringAngleToTw(1.0, 0.0), 0.0, 1e-6); + + // Forward, positive steering angle → positive tw + // tw = tan(angle) * |v| / wheelbase + double sa = 0.3; + double v = 1.0; + double expected_tw = std::tan(sa) * std::abs(v) / WHEELBASE; + EXPECT_NEAR(velocity_polygon_->callSteeringAngleToTw(v, sa), expected_tw, 1e-6); + + // Reverse, positive steering angle → negative tw (sign-corrected for reverse) + double expected_tw_reverse = -(std::tan(sa) * std::abs(-v) / WHEELBASE); + EXPECT_NEAR(velocity_polygon_->callSteeringAngleToTw(-v, sa), expected_tw_reverse, 1e-6); +} + +TEST_F(Tester, testIsInRangeWithSteeringWheelSpeed) +{ + // Setup: 2 speed fields with steering angle params + // Slow: 0.0 to 0.5 steering wheel speed, steering angle -0.5 to 0.5 + // Fast: 0.5 to 1.0 steering wheel speed, steering angle -0.5 to 0.5 + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"slow", "fast"}); + addSteeringAngleSubPolygon("slow", 0.0, 0.5, -0.5, 0.5, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("fast", 0.5, 1.0, -0.5, 0.5, STEERING_POLYGON_FAST_STR); + createSteeringVelocityPolygon("limit"); + + // With zero steering angle: baselink speed == steering wheel speed + // 0.3 m/s baselink → 0.3 steering → should be "slow" field + nav2_collision_monitor::Velocity vel{0.3, 0.0, 0.0}; + velocity_polygon_->updatePolygon(vel); + EXPECT_EQ(velocity_polygon_->getCurrentSubPolygonName(), "slow"); + + // 0.7 m/s baselink → 0.7 steering → should be "fast" field + vel = {0.7, 0.0, 0.0}; + velocity_polygon_->updatePolygon(vel); + EXPECT_EQ(velocity_polygon_->getCurrentSubPolygonName(), "fast"); + + // With non-zero steering angle, the baselink speed that maps to a given + // steering wheel speed is lower. E.g., at 0.3 rad steering angle: + // v_steering = v_baselink / cos(0.3) ≈ v_baselink / 0.9553 + // So 0.48 baselink → 0.48 / 0.9553 ≈ 0.502 steering → "fast" field + // We need to set tw correctly: tw = tan(sa) * |v| / wheelbase + double sa = 0.3; + double v_base = 0.48; + double tw = std::tan(sa) * std::abs(v_base) / WHEELBASE; + vel = {v_base, 0.0, tw}; + velocity_polygon_->updatePolygon(vel); + EXPECT_EQ(velocity_polygon_->getCurrentSubPolygonName(), "fast"); + + // With same steering angle, 0.45 baselink → 0.45 / 0.9553 ≈ 0.471 steering → "slow" field + v_base = 0.45; + tw = std::tan(sa) * std::abs(v_base) / WHEELBASE; + vel = {v_base, 0.0, tw}; + velocity_polygon_->updatePolygon(vel); + EXPECT_EQ(velocity_polygon_->getCurrentSubPolygonName(), "slow"); +} + +TEST_F(Tester, testLinearLimitConversionInUpdatePolygon) +{ + // Setup: create a LIMIT velocity polygon with steering angle + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"slow"}); + + // Add a sub-polygon with linear_limit = 0.49 (steering wheel speed) + const std::string prefix = std::string(POLYGON_NAME) + ".slow"; + test_node_->declare_parameter(prefix + ".points", rclcpp::ParameterValue( + std::string(STEERING_POLYGON_SLOW_STR))); + test_node_->set_parameter(rclcpp::Parameter(prefix + ".points", + std::string(STEERING_POLYGON_SLOW_STR))); + test_node_->declare_parameter(prefix + ".linear_min", rclcpp::ParameterValue(0.0)); + test_node_->set_parameter(rclcpp::Parameter(prefix + ".linear_min", 0.0)); + test_node_->declare_parameter(prefix + ".linear_max", rclcpp::ParameterValue(1.0)); + test_node_->set_parameter(rclcpp::Parameter(prefix + ".linear_max", 1.0)); + test_node_->declare_parameter(prefix + ".steering_angle_min", rclcpp::ParameterValue(-0.5)); + test_node_->set_parameter(rclcpp::Parameter(prefix + ".steering_angle_min", -0.5)); + test_node_->declare_parameter(prefix + ".steering_angle_max", rclcpp::ParameterValue(0.5)); + test_node_->set_parameter(rclcpp::Parameter(prefix + ".steering_angle_max", 0.5)); + test_node_->declare_parameter(prefix + ".linear_limit", rclcpp::ParameterValue(0.49)); + test_node_->set_parameter(rclcpp::Parameter(prefix + ".linear_limit", 0.49)); + test_node_->declare_parameter(prefix + ".angular_limit", rclcpp::ParameterValue(0.5)); + test_node_->set_parameter(rclcpp::Parameter(prefix + ".angular_limit", 0.5)); + + createSteeringVelocityPolygon("limit"); + + // At zero steering angle, linear_limit should be 0.49 * cos(0) = 0.49 + nav2_collision_monitor::Velocity vel{0.3, 0.0, 0.0}; + velocity_polygon_->updatePolygon(vel); + EXPECT_NEAR(velocity_polygon_->getLinearLimit(), 0.49, 1e-6); + + // At steering angle 0.3 rad, linear_limit should be 0.49 * cos(0.3) ≈ 0.4681 + double sa = 0.3; + double v_base = 0.3; + double tw = std::tan(sa) * std::abs(v_base) / WHEELBASE; + vel = {v_base, 0.0, tw}; + velocity_polygon_->updatePolygon(vel); + double expected_limit = 0.49 * std::cos(sa); + EXPECT_NEAR(velocity_polygon_->getLinearLimit(), expected_limit, 1e-3); +} + +TEST_F(Tester, testIsPointInsidePoly) +{ + // Simple square polygon: (-1,-1), (1,-1), (1,1), (-1,1) + std::vector square = { + {-1.0, -1.0}, {1.0, -1.0}, {1.0, 1.0}, {-1.0, 1.0} + }; + + // Point inside + EXPECT_TRUE(VelocityPolygonWrapper::callIsPointInsidePoly({0.0, 0.0}, square)); + EXPECT_TRUE(VelocityPolygonWrapper::callIsPointInsidePoly({0.5, 0.5}, square)); + + // Point outside + EXPECT_FALSE(VelocityPolygonWrapper::callIsPointInsidePoly({2.0, 0.0}, square)); + EXPECT_FALSE(VelocityPolygonWrapper::callIsPointInsidePoly({0.0, 2.0}, square)); + EXPECT_FALSE(VelocityPolygonWrapper::callIsPointInsidePoly({-2.0, -2.0}, square)); +} + +TEST_F(Tester, testGetPointsInsideSubPolygon) +{ + // Setup a steering velocity polygon + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"slow"}); + // Use a polygon that covers (-0.3,-0.3) to (0.5,0.3) + addSteeringAngleSubPolygon("slow", 0.0, 0.5, -0.5, 0.5, STEERING_POLYGON_SLOW_STR); + createSteeringVelocityPolygon("limit"); + + auto sub_polygons = velocity_polygon_->getSubPolygons(); + ASSERT_EQ(sub_polygons.size(), 1u); + + // Create collision points: some inside, some outside + std::unordered_map> collision_map; + collision_map["source"] = { + {0.0, 0.0}, // inside + {0.2, 0.1}, // inside + {10.0, 10.0}, // outside + {-10.0, -10.0} // outside + }; + + int count = velocity_polygon_->callGetPointsInsideSubPolygon(sub_polygons[0], collision_map); + EXPECT_EQ(count, 2); +} + +TEST_F(Tester, testFindField) +{ + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"slow", "fast"}); + addSteeringAngleSubPolygon("slow", 0.0, 0.5, -0.5, 0.5, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("fast", 0.5, 1.0, -0.5, 0.5, STEERING_POLYGON_FAST_STR); + createSteeringVelocityPolygon("limit"); + + // Find field for 0.3 speed at 0 angle → slow + auto field = velocity_polygon_->callFindField(0.3, 0.0); + ASSERT_NE(field, nullptr); + EXPECT_EQ(field->velocity_polygon_name_, "slow"); + + // Find field for 0.7 speed at 0 angle → fast + field = velocity_polygon_->callFindField(0.7, 0.0); + ASSERT_NE(field, nullptr); + EXPECT_EQ(field->velocity_polygon_name_, "fast"); + + // Out of range speed + field = velocity_polygon_->callFindField(1.5, 0.0); + EXPECT_EQ(field, nullptr); + + // Out of range angle + field = velocity_polygon_->callFindField(0.3, 1.0); + EXPECT_EQ(field, nullptr); +} + +TEST_F(Tester, testFindFieldsForAngle) +{ + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"slow", "fast"}); + addSteeringAngleSubPolygon("slow", 0.0, 0.5, -0.5, 0.5, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("fast", 0.5, 1.0, -0.5, 0.5, STEERING_POLYGON_FAST_STR); + createSteeringVelocityPolygon("limit"); + + // At angle 0, both forward fields should match, sorted by linear_min ascending + auto fields = velocity_polygon_->callFindFieldsForAngle(0.0, true); + ASSERT_EQ(fields.size(), 2u); + EXPECT_EQ(fields[0]->velocity_polygon_name_, "slow"); + EXPECT_EQ(fields[1]->velocity_polygon_name_, "fast"); + + // At angle 1.0 (outside range), no fields + fields = velocity_polygon_->callFindFieldsForAngle(1.0, true); + EXPECT_EQ(fields.size(), 0u); +} + +// ==================== validateSteering tests ==================== + +TEST_F(Tester, testValidateSteeringDirectionReversalHighSpeed) +{ + // Setup: 2 speed fields with steering angle + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"slow", "fast"}); + addSteeringAngleSubPolygon("slow", 0.0, 0.5, -0.5, 0.5, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("fast", 0.5, 1.0, -0.5, 0.5, STEERING_POLYGON_FAST_STR); + createSteeringVelocityPolygon("limit"); + + // Update polygon first so internal state is set + nav2_collision_monitor::Velocity vel{0.3, 0.0, 0.0}; + velocity_polygon_->updatePolygon(vel); + + // Target: forward, Current: backward (speed crosses zero), |current| > threshold + nav2_collision_monitor::Velocity cmd_vel{0.5, 0.0, 0.3}; // forward target with steering + nav2_collision_monitor::Velocity odom_vel{-0.5, 0.0, 0.1}; // moving backward + std::unordered_map> collision_map; + collision_map["source"] = {}; + + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->validateSteering(cmd_vel, odom_vel, collision_map, action); + EXPECT_TRUE(modified); + // Should clamp tw to maintain current steering angle + EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); +} + +TEST_F(Tester, testValidateSteeringDirectionReversalLowSpeed) +{ + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"slow"}); + addSteeringAngleSubPolygon("slow", 0.0, 0.5, -0.5, 0.5, STEERING_POLYGON_SLOW_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity vel{0.05, 0.0, 0.0}; + velocity_polygon_->updatePolygon(vel); + + // Target: forward, Current: backward but below threshold → allow steering freely + nav2_collision_monitor::Velocity cmd_vel{0.3, 0.0, 0.1}; + nav2_collision_monitor::Velocity odom_vel{-0.05, 0.0, 0.0}; // below threshold + std::unordered_map> collision_map; + collision_map["source"] = {}; + + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->validateSteering(cmd_vel, odom_vel, collision_map, action); + EXPECT_FALSE(modified); // steering allowed freely +} + +TEST_F(Tester, testValidateSteeringBothBelowThreshold) +{ + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"slow"}); + addSteeringAngleSubPolygon("slow", 0.0, 0.5, -0.5, 0.5, STEERING_POLYGON_SLOW_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity vel{0.05, 0.0, 0.0}; + velocity_polygon_->updatePolygon(vel); + + // Both target and current below threshold → done (no modification) + nav2_collision_monitor::Velocity cmd_vel{0.05, 0.0, 0.02}; + nav2_collision_monitor::Velocity odom_vel{0.05, 0.0, 0.01}; + std::unordered_map> collision_map; + collision_map["source"] = {}; + + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->validateSteering(cmd_vel, odom_vel, collision_map, action); + EXPECT_FALSE(modified); +} + +TEST_F(Tester, testValidateSteeringSameBucketResultStaysInField) +{ + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"slow", "fast"}); + addSteeringAngleSubPolygon("slow", 0.0, 0.5, -0.5, 0.5, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("fast", 0.5, 1.0, -0.5, 0.5, STEERING_POLYGON_FAST_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity vel{0.3, 0.0, 0.0}; + velocity_polygon_->updatePolygon(vel); + + // Same field, result velocity stays in same field, no collision check needed → no modification + nav2_collision_monitor::Velocity cmd_vel{0.4, 0.0, 0.0}; // target in slow field + nav2_collision_monitor::Velocity odom_vel{0.2, 0.0, 0.0}; // current in slow field + std::unordered_map> collision_map; + collision_map["source"] = {}; // no collision points + + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->validateSteering(cmd_vel, odom_vel, collision_map, action); + EXPECT_FALSE(modified); +} + +TEST_F(Tester, testValidateSteeringSameBucketFasterFieldCollision) +{ + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"slow", "fast"}); + addSteeringAngleSubPolygon("slow", 0.0, 0.5, -0.5, 0.5, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("fast", 0.5, 1.0, -0.5, 0.5, STEERING_POLYGON_FAST_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity vel{0.3, 0.0, 0.0}; + velocity_polygon_->updatePolygon(vel); + + // Same field (cmd_vel maps to slow field), but result velocity from prior processing + // falls in the next faster (fast) field which has collision → limit speed. + // The steering check sees result_vel is in a different field than current, checks + // collision there, and caps speed to current field's linear_max. + nav2_collision_monitor::Velocity cmd_vel{0.4, 0.0, 0.0}; // target in slow field + nav2_collision_monitor::Velocity odom_vel{0.2, 0.0, 0.0}; // current in slow field + std::unordered_map> collision_map; + // Place collision points inside the fast polygon + collision_map["source"] = { + {1.2, 0.0}, // inside fast polygon + {1.3, 0.1}, // inside fast polygon + }; + + // Set robot_action with a velocity that exceeds the slow field max + // (simulating that the main loop allowed higher speed) + nav2_collision_monitor::Velocity action_vel{0.6, 0.0, 0.0}; + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, action_vel, ""}; + + bool modified = velocity_polygon_->validateSteering(cmd_vel, odom_vel, collision_map, action); + EXPECT_TRUE(modified); + EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); + // Speed should be limited to slow field's linear_max in baselink speed + // At 0 steering angle, that's just 0.5 + EXPECT_LE(std::abs(action.req_vel.x), 0.5 + 1e-6); +} + +TEST_F(Tester, testValidateSteeringDifferentBucketCollisionFree) +{ + // Fields at different steering angles + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, + {"straight_slow", "straight_fast", "left_slow", "left_fast"}); + addSteeringAngleSubPolygon("straight_slow", 0.0, 0.5, -0.1, 0.1, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("straight_fast", 0.5, 1.0, -0.1, 0.1, STEERING_POLYGON_FAST_STR); + addSteeringAngleSubPolygon("left_slow", 0.0, 0.5, 0.1, 0.5, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("left_fast", 0.5, 1.0, 0.1, 0.5, STEERING_POLYGON_FAST_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity vel{0.3, 0.0, 0.0}; + velocity_polygon_->updatePolygon(vel); + + // Current: straight at 0.3, Target: left at 0.3 + nav2_collision_monitor::Velocity odom_vel{0.3, 0.0, 0.0}; // straight + double target_sa = 0.3; + double target_tw = std::tan(target_sa) * 0.3 / WHEELBASE; + nav2_collision_monitor::Velocity cmd_vel{0.3, 0.0, target_tw}; + + std::unordered_map> collision_map; + collision_map["source"] = {}; // no collision + + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->validateSteering(cmd_vel, odom_vel, collision_map, action); + EXPECT_FALSE(modified); // fastest field is collision-free → done +} + +TEST_F(Tester, testValidateSteeringDifferentBucketAllCollision) +{ + // Fields at different steering angles + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, + {"straight_slow", "left_slow"}); + addSteeringAngleSubPolygon("straight_slow", 0.0, 0.5, -0.1, 0.1, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("left_slow", 0.0, 0.5, 0.1, 0.5, STEERING_POLYGON_SLOW_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity vel{0.3, 0.0, 0.0}; + velocity_polygon_->updatePolygon(vel); + + // Current: straight at 0.3, Target: left at 0.3 + nav2_collision_monitor::Velocity odom_vel{0.3, 0.0, 0.0}; + double target_sa = 0.3; + double target_tw = std::tan(target_sa) * 0.3 / WHEELBASE; + nav2_collision_monitor::Velocity cmd_vel{0.3, 0.0, target_tw}; + + std::unordered_map> collision_map; + // Collision in all left fields + collision_map["source"] = { + {0.0, 0.0}, + {0.2, 0.1}, + }; + + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->validateSteering(cmd_vel, odom_vel, collision_map, action); + EXPECT_TRUE(modified); + // All fields in collision → use slowest field (allowed even if in collision), LIMIT not STOP + EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); + // Speed should be limited to slowest field's linear_max (0.5) converted to baselink + // At the neighbour bucket boundary (0.1 rad) + double neighbour_angle = 0.1; // straight bucket boundary toward left + double expected_max = 0.5 * std::cos(neighbour_angle); + EXPECT_LE(std::abs(action.req_vel.x), expected_max + 1e-3); +} + +TEST_F(Tester, testValidateSteeringDifferentBucketDecelerationToValidField) +{ + // Setup: straight and left, each with slow and fast fields + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, + {"straight_slow", "straight_fast", "left_slow", "left_fast"}); + addSteeringAngleSubPolygon("straight_slow", 0.0, 0.5, -0.1, 0.1, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("straight_fast", 0.5, 1.0, -0.1, 0.1, STEERING_POLYGON_FAST_STR); + addSteeringAngleSubPolygon("left_slow", 0.0, 0.5, 0.1, 0.5, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("left_fast", 0.5, 1.0, 0.1, 0.5, STEERING_POLYGON_FAST_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity vel{0.7, 0.0, 0.0}; + velocity_polygon_->updatePolygon(vel); + + // Current: straight at 0.7 (fast field), Target: left at 0.7 (fast field) + nav2_collision_monitor::Velocity odom_vel{0.7, 0.0, 0.0}; + double target_sa = 0.3; + double target_tw = std::tan(target_sa) * 0.7 / WHEELBASE; + nav2_collision_monitor::Velocity cmd_vel{0.7, 0.0, target_tw}; + + std::unordered_map> collision_map; + // Collision in the fast left polygon only (points inside the large polygon) + collision_map["source"] = { + {1.2, 0.5}, + {1.3, 0.6}, + }; + + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->validateSteering(cmd_vel, odom_vel, collision_map, action); + EXPECT_TRUE(modified); + EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); + // Speed should be limited to slow field's linear_max (0.5) converted to baselink + // At the neighbour bucket boundary (0.1 rad), baselink = 0.5 * cos(0.1) + double neighbour_angle = 0.1; // straight bucket boundary toward left + double expected_max = 0.5 * std::cos(neighbour_angle); + EXPECT_LE(std::abs(action.req_vel.x), expected_max + 1e-3); +} + +TEST_F(Tester, testValidateSteeringBackwardSameBucketFasterFieldCollision) +{ + // Two backward fields in the same bucket: + // backward_slow: [-0.3, 0.0], backward_mid: [-0.7, -0.3] + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, + {"backward_slow", "backward_mid"}); + addSteeringAngleSubPolygon("backward_slow", -0.3, 0.0, -0.5, 0.5, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("backward_mid", -0.7, -0.3, -0.5, 0.5, STEERING_POLYGON_FAST_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity vel{-0.2, 0.0, 0.0}; + velocity_polygon_->updatePolygon(vel); + + // cmd_vel and odom both in backward_slow field + nav2_collision_monitor::Velocity cmd_vel{-0.2, 0.0, 0.0}; + nav2_collision_monitor::Velocity odom_vel{-0.1, 0.0, 0.0}; + std::unordered_map> collision_map; + // Collision points inside the backward_mid (larger) polygon + collision_map["source"] = { + {-0.6, 0.0}, + {-0.7, 0.1}, + }; + + // result velocity from prior processing falls in backward_mid field + nav2_collision_monitor::Velocity action_vel{-0.5, 0.0, 0.0}; + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, action_vel, ""}; + + bool modified = velocity_polygon_->validateSteering(cmd_vel, odom_vel, collision_map, action); + EXPECT_TRUE(modified); + EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); + // Speed should be limited to backward_slow's linear_min (-0.3) converted to baselink + // At 0 steering angle, that's -0.3 + EXPECT_GE(action.req_vel.x, -0.3 - 1e-6); // not more negative than -0.3 +} + +TEST_F(Tester, testValidateSteeringNotApplicableToNonSteering) +{ + // Create a normal theta-based velocity polygon (not steering angle) + createVelocityPolygon("stop", IS_NOT_HOLONOMIC); + + nav2_collision_monitor::Velocity cmd_vel{0.3, 0.0, 0.0}; + nav2_collision_monitor::Velocity odom_vel{0.2, 0.0, 0.0}; + std::unordered_map> collision_map; + collision_map["source"] = {}; + + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->validateSteering(cmd_vel, odom_vel, collision_map, action); + EXPECT_FALSE(modified); // Should not apply to theta-based polygons +} + + int main(int argc, char ** argv) { // Initialize the system From cae707ba73f3ee0ce866e2ac09f111e1f8ae6c89 Mon Sep 17 00:00:00 2001 From: Johannes Plapp Date: Wed, 11 Mar 2026 16:25:17 +0100 Subject: [PATCH 02/35] debugging --- .../velocity_polygon.hpp | 3 + .../src/collision_monitor_node.cpp | 32 ++- .../src/velocity_polygon.cpp | 228 +++++++++++++----- nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/msg/SteeringValidationDebug.msg | 74 ++++++ 5 files changed, 258 insertions(+), 80 deletions(-) create mode 100644 nav2_msgs/msg/SteeringValidationDebug.msg diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp index 02cb8b8a5e0..4ef1daf72b3 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp @@ -26,6 +26,7 @@ #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" @@ -206,6 +207,8 @@ class VelocityPolygon : public Polygon // Clock rclcpp::Clock::SharedPtr clock_; + // Debug publisher for steering validation + rclcpp::Publisher::SharedPtr steering_debug_pub_; // Current subpolygon name std::string current_subpolygon_name_; // Variables diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index f9537aa93fc..218b75a14fc 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -501,6 +501,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 active_limit_vel_polygon; + for (std::shared_ptr polygon : polygons_) { if (!polygon->getEnabled() || !enabled_) { continue; @@ -531,6 +533,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(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 @@ -547,19 +557,15 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: } } - // Step 2: Steering validation - if (enable_steering_validation_) { - for (auto polygon : polygons_) { - auto vel_polygon = std::dynamic_pointer_cast(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->validateSteering( - cmd_vel_in, odom_vel, sources_collision_points_map, robot_action)) - { - action_polygon = polygon; - } + // 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; } } diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index bdd68e7639f..370d904102a 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -16,6 +16,7 @@ #include #include +#include #include "nav2_ros_common/node_utils.hpp" @@ -205,6 +206,10 @@ bool VelocityPolygon::getParameters( ex.what()); return false; } + + steering_debug_pub_ = node->create_publisher( + "~/steering_validation_debug", rclcpp::QoS(1)); + return true; } @@ -393,7 +398,7 @@ VelocityPolygon::findFieldsForAngle(double steering_angle, bool forward) const continue; } if (steering_angle >= sp.steering_angle_min_ && steering_angle <= sp.steering_angle_max_) { - if (forward ? (sp.linear_max_ >= 0) : (sp.linear_min_ <= 0)) { + if (forward ? (sp.linear_max_ > 0) : (sp.linear_min_ < 0)) { result.push_back(&sp); } } @@ -461,20 +466,112 @@ bool VelocityPolygon::validateSteering( return false; } + nav2_msgs::msg::SteeringValidationDebug debug_msg; + debug_msg.header.stamp = clock_->now(); + debug_msg.polygon_name = polygon_name_; + debug_msg.steering_angle_limit = std::numeric_limits::quiet_NaN(); + debug_msg.speed_limit_applied = 0.0; + debug_msg.next_field_collision_pts = -1; + debug_msg.neighbour_collision_pts = -1; + + // Step 1 diagnostics: re-check the polygon state from processStopSlowdownLimit + debug_msg.step1_active_sub_polygon = current_subpolygon_name_; + debug_msg.step1_shape_set = isShapeSet(); + debug_msg.step1_min_points = min_points_; + debug_msg.step1_points_inside = getPointsInside(collision_points_map); + debug_msg.step1_linear_limit = linear_limit_; + debug_msg.step1_angular_limit = angular_limit_; + debug_msg.step1_action_type = robot_action.action_type; + debug_msg.step1_req_vel_x = robot_action.req_vel.x; + debug_msg.step1_req_vel_y = robot_action.req_vel.y; + debug_msg.step1_req_vel_tw = robot_action.req_vel.tw; + + // Source filtering diagnostics + debug_msg.step1_configured_sources = getSourcesNames(); + debug_msg.step1_polygon_vertex_count = static_cast(poly_.size()); + int configured_pts = 0; + for (const auto & src_name : debug_msg.step1_configured_sources) { + auto it = collision_points_map.find(src_name); + if (it != collision_points_map.end()) { + configured_pts += static_cast(it->second.size()); + } + } + debug_msg.step1_configured_source_pts = configured_pts; + int all_pts = 0; + for (const auto & kv : collision_points_map) { + all_pts += static_cast(kv.second.size()); + } + debug_msg.step1_all_source_pts = all_pts; + + // Bounding box of polygon vertices + double poly_min_x = std::numeric_limits::max(); + double poly_max_x = std::numeric_limits::lowest(); + double poly_min_y = std::numeric_limits::max(); + double poly_max_y = std::numeric_limits::lowest(); + for (const auto & v : poly_) { + poly_min_x = std::min(poly_min_x, v.x); + poly_max_x = std::max(poly_max_x, v.x); + poly_min_y = std::min(poly_min_y, v.y); + poly_max_y = std::max(poly_max_y, v.y); + } + debug_msg.step1_poly_min_x = poly_min_x; + debug_msg.step1_poly_max_x = poly_max_x; + debug_msg.step1_poly_min_y = poly_min_y; + debug_msg.step1_poly_max_y = poly_max_y; + + // Bounding box of collision points from configured sources + double pts_min_x = std::numeric_limits::max(); + double pts_max_x = std::numeric_limits::lowest(); + double pts_min_y = std::numeric_limits::max(); + double pts_max_y = std::numeric_limits::lowest(); + for (const auto & src_name : debug_msg.step1_configured_sources) { + auto it = collision_points_map.find(src_name); + if (it != collision_points_map.end()) { + for (const auto & pt : it->second) { + pts_min_x = std::min(pts_min_x, pt.x); + pts_max_x = std::max(pts_max_x, pt.x); + pts_min_y = std::min(pts_min_y, pt.y); + pts_max_y = std::max(pts_max_y, pt.y); + } + } + } + debug_msg.step1_pts_min_x = pts_min_x; + debug_msg.step1_pts_max_x = pts_max_x; + debug_msg.step1_pts_min_y = pts_min_y; + debug_msg.step1_pts_max_y = pts_max_y; + + // Count points within the polygon's bounding box + int pts_in_bbox = 0; + for (const auto & src_name : debug_msg.step1_configured_sources) { + auto it = collision_points_map.find(src_name); + if (it != collision_points_map.end()) { + for (const auto & pt : it->second) { + if (pt.x >= poly_min_x && pt.x <= poly_max_x && + pt.y >= poly_min_y && pt.y <= poly_max_y) + { + pts_in_bbox++; + } + } + } + } + debug_msg.step1_pts_in_bbox = pts_in_bbox; + const double target_speed = cmd_vel_in.x; const double current_speed = odom_vel.x; const double target_steering_angle = computeSteeringAngle(cmd_vel_in); const double current_sa = computeSteeringAngle(odom_vel); - RCLCPP_INFO( - logger_, - "[%s] validateSteering: target_speed=%.3f, current_speed=%.3f, " - "target_sa=%.3f, current_sa=%.3f, cmd_vel_in=(%.3f, %.3f, %.3f), odom_vel=(%.3f, %.3f, %.3f)", - polygon_name_.c_str(), target_speed, current_speed, - target_steering_angle, current_sa, - cmd_vel_in.x, cmd_vel_in.y, cmd_vel_in.tw, - odom_vel.x, odom_vel.y, odom_vel.tw); + debug_msg.target_speed = target_speed; + debug_msg.current_speed = current_speed; + debug_msg.target_steering_angle = target_steering_angle; + debug_msg.current_steering_angle = current_sa; + debug_msg.cmd_vel_x = cmd_vel_in.x; + debug_msg.cmd_vel_y = cmd_vel_in.y; + debug_msg.cmd_vel_tw = cmd_vel_in.tw; + debug_msg.odom_vel_x = odom_vel.x; + debug_msg.odom_vel_y = odom_vel.y; + debug_msg.odom_vel_tw = odom_vel.tw; bool modified = false; Velocity result_vel = robot_action.req_vel; @@ -482,17 +579,14 @@ bool VelocityPolygon::validateSteering( // Check if speed crosses zero (direction reversal) bool crosses_zero = (target_speed > 0 && current_speed < 0) || (target_speed < 0 && current_speed > 0); + debug_msg.crosses_zero = crosses_zero; if (crosses_zero) { if (std::abs(current_speed) > low_speed_threshold_) { // Must decelerate first — clamp tw to maintain current steering angle result_vel.tw = steeringAngleToTw(result_vel.x, current_sa); modified = true; - RCLCPP_INFO( - logger_, - "[%s] validateSteering: direction reversal detected, clamping tw to maintain " - "current_sa=%.3f (current_speed=%.3f > threshold=%.3f)", - polygon_name_.c_str(), current_sa, current_speed, low_speed_threshold_); + debug_msg.steering_angle_limit = current_sa; } // else: abs(current) < threshold → allow steering freely if (modified) { @@ -500,38 +594,42 @@ bool VelocityPolygon::validateSteering( robot_action.polygon_name = polygon_name_; robot_action.action_type = LIMIT; } + debug_msg.modified = modified; + debug_msg.result_vel_x = result_vel.x; + debug_msg.result_vel_y = result_vel.y; + debug_msg.result_vel_tw = result_vel.tw; + steering_debug_pub_->publish(debug_msg); return modified; } // Speed does NOT cross zero // 1. Both abs(target) and abs(current) below threshold → done - if (std::abs(target_speed) < low_speed_threshold_ && - std::abs(current_speed) < low_speed_threshold_) - { + bool both_below = std::abs(target_speed) < low_speed_threshold_ && + std::abs(current_speed) < low_speed_threshold_; + debug_msg.both_below_threshold = both_below; + if (both_below) { + debug_msg.modified = false; + debug_msg.result_vel_x = result_vel.x; + debug_msg.result_vel_y = result_vel.y; + debug_msg.result_vel_tw = result_vel.tw; + steering_debug_pub_->publish(debug_msg); return false; } double target_sw_speed = baselinkToSteeringSpeed(cmd_vel_in.x, cmd_vel_in.tw); double current_sw_speed = baselinkToSteeringSpeed(odom_vel.x, odom_vel.tw); + debug_msg.target_sw_speed = target_sw_speed; + debug_msg.current_sw_speed = current_sw_speed; const SubPolygonParameter * current_field = findField(current_sw_speed, current_sa); + debug_msg.current_field_name = current_field ? current_field->velocity_polygon_name_ : ""; - RCLCPP_INFO( - logger_, - "[%s] validateSteering: target_sw_speed=%.3f, current_sw_speed=%.3f, " - "current_field=%s", - polygon_name_.c_str(), target_sw_speed, current_sw_speed, - current_field ? current_field->velocity_polygon_name_.c_str() : "null"); // 2. Check if target angle is in same bucket (angle range) as current bool same_bucket = current_field != nullptr && target_steering_angle >= current_field->steering_angle_min_ && target_steering_angle <= current_field->steering_angle_max_; - - RCLCPP_INFO( - logger_, - "[%s] validateSteering: same_bucket=%s", - polygon_name_.c_str(), same_bucket ? "true" : "false"); + debug_msg.same_bucket = same_bucket; if (same_bucket) { // If result velocity falls into some faster field (same bucket), check the @@ -546,19 +644,17 @@ bool VelocityPolygon::validateSteering( for (size_t i = 0; i < fields_at_angle.size(); i++) { if (fields_at_angle[i] == current_field && i + 1 < fields_at_angle.size()) { const SubPolygonParameter * next_field = fields_at_angle[i + 1]; - if (getPointsInsideSubPolygon(*next_field, collision_points_map) >= min_points_) { + debug_msg.next_field_name = next_field->velocity_polygon_name_; + int pts = getPointsInsideSubPolygon(*next_field, collision_points_map); + debug_msg.next_field_collision_pts = pts; + if (pts >= min_points_) { // Forward: limit to linear_max (upper bound) // Backward: limit to linear_min (lower bound, more negative = faster) double limit_sw = (current_sw_speed >= 0) ? current_field->linear_max_ : current_field->linear_min_; double max_baselink = steeringToBaselinkSpeed(limit_sw, current_sa); if (std::abs(result_vel.x) > std::abs(max_baselink)) { - RCLCPP_INFO( - logger_, - "[%s] validateSteering: same_bucket speed limit — next field '%s' in collision, " - "limiting vel.x from %.3f to %.3f (sw_limit=%.3f)", - polygon_name_.c_str(), next_field->velocity_polygon_name_.c_str(), - result_vel.x, max_baselink, limit_sw); + debug_msg.speed_limit_applied = max_baselink; result_vel.x = max_baselink; result_vel.tw = steeringAngleToTw(result_vel.x, target_steering_angle); modified = true; @@ -574,15 +670,16 @@ bool VelocityPolygon::validateSteering( robot_action.polygon_name = polygon_name_; robot_action.action_type = LIMIT; } + debug_msg.modified = modified; + debug_msg.result_vel_x = result_vel.x; + debug_msg.result_vel_y = result_vel.y; + debug_msg.result_vel_tw = result_vel.tw; + steering_debug_pub_->publish(debug_msg); return modified; } // 3. Different bucket — find neighbouring bucket (one step in steering direction) - RCLCPP_INFO( - logger_, - "[%s] validateSteering: different bucket — target_sa=%.3f outside current field [%.3f, %.3f]", - polygon_name_.c_str(), target_steering_angle, - current_field->steering_angle_min_, current_field->steering_angle_max_); + double neighbour_angle; if (target_steering_angle > current_sa) { neighbour_angle = current_field->steering_angle_max_; @@ -597,6 +694,11 @@ bool VelocityPolygon::validateSteering( bool forward = target_sw_speed >= 0; auto neighbour_fields = findFieldsForAngle(lookup_angle, forward); if (neighbour_fields.empty()) { + debug_msg.modified = false; + debug_msg.result_vel_x = result_vel.x; + debug_msg.result_vel_y = result_vel.y; + debug_msg.result_vel_tw = result_vel.tw; + steering_debug_pub_->publish(debug_msg); return false; } @@ -622,7 +724,15 @@ bool VelocityPolygon::validateSteering( } // Check if fastest field is collision-free → done - if (getPointsInsideSubPolygon(*neighbour_fields[start_idx], collision_points_map) < min_points_) { + int start_pts = getPointsInsideSubPolygon( + *neighbour_fields[start_idx], collision_points_map); + debug_msg.neighbour_collision_pts = start_pts; + if (start_pts < min_points_) { + debug_msg.modified = false; + debug_msg.result_vel_x = result_vel.x; + debug_msg.result_vel_y = result_vel.y; + debug_msg.result_vel_tw = result_vel.tw; + steering_debug_pub_->publish(debug_msg); return false; } @@ -638,17 +748,9 @@ bool VelocityPolygon::validateSteering( // All fields in collision — use the slowest field in the target direction // (allowed even if in collision) valid_field = neighbour_fields[0]; // sorted ascending, index 0 is slowest - RCLCPP_INFO( - logger_, - "[%s] validateSteering: all neighbour fields in collision, " - "falling back to slowest field '%s'", - polygon_name_.c_str(), valid_field->velocity_polygon_name_.c_str()); - } else { - RCLCPP_INFO( - logger_, - "[%s] validateSteering: valid neighbour field found: '%s'", - polygon_name_.c_str(), valid_field->velocity_polygon_name_.c_str()); + } + debug_msg.valid_field_name = valid_field->velocity_polygon_name_; // 4. Adapt speed and steering angle // 4a. Limit target speed to valid field's speed boundary (converted to baselink) @@ -658,12 +760,7 @@ bool VelocityPolygon::validateSteering( double valid_max_baselink = steeringToBaselinkSpeed( valid_limit_sw, neighbour_angle); if (std::abs(result_vel.x) > std::abs(valid_max_baselink)) { - RCLCPP_INFO( - logger_, - "[%s] validateSteering: limiting speed from %.3f to %.3f " - "(valid_limit_sw=%.3f, neighbour_angle=%.3f)", - polygon_name_.c_str(), result_vel.x, valid_max_baselink, - valid_limit_sw, neighbour_angle); + debug_msg.speed_limit_applied = valid_max_baselink; result_vel.x = valid_max_baselink; } @@ -677,22 +774,19 @@ bool VelocityPolygon::validateSteering( } else { limited_sa = current_field->steering_angle_min_; } - RCLCPP_INFO( - logger_, - "[%s] validateSteering: limiting steering angle to %.3f " - "(current_speed=%.3f > valid_max=%.3f)", - polygon_name_.c_str(), limited_sa, current_baselink_abs, valid_max_baselink_abs); + debug_msg.steering_angle_limit = limited_sa; result_vel.tw = steeringAngleToTw(result_vel.x, limited_sa); } - RCLCPP_INFO( - logger_, - "[%s] validateSteering: final result_vel=(%.3f, %.3f, %.3f)", - polygon_name_.c_str(), result_vel.x, result_vel.y, result_vel.tw); - robot_action.req_vel = result_vel; robot_action.polygon_name = polygon_name_; robot_action.action_type = LIMIT; + + debug_msg.modified = true; + debug_msg.result_vel_x = result_vel.x; + debug_msg.result_vel_y = result_vel.y; + debug_msg.result_vel_tw = result_vel.tw; + steering_debug_pub_->publish(debug_msg); return true; } diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index aa7c8951f19..542ee4cb7ee 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -44,6 +44,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/TrajectoryPoint.msg" "msg/VelocityPolygonPair.msg" "msg/ActiveVelocityPolygons.msg" + "msg/SteeringValidationDebug.msg" "srv/GetCosts.srv" "msg/PolygonObject.msg" "msg/CircleObject.msg" diff --git a/nav2_msgs/msg/SteeringValidationDebug.msg b/nav2_msgs/msg/SteeringValidationDebug.msg new file mode 100644 index 00000000000..6cd759b6f8b --- /dev/null +++ b/nav2_msgs/msg/SteeringValidationDebug.msg @@ -0,0 +1,74 @@ +# Debug output from VelocityPolygon::validateSteering +std_msgs/Header header + +# Input velocities +float64 target_speed # cmd_vel_in.x (baselink) +float64 current_speed # odom_vel.x (baselink) +float64 target_steering_angle # computed from cmd_vel_in +float64 current_steering_angle # computed from odom_vel +float64 target_sw_speed # target steering wheel speed +float64 current_sw_speed # current steering wheel speed + +# cmd_vel_in components +float64 cmd_vel_x +float64 cmd_vel_y +float64 cmd_vel_tw + +# odom_vel components +float64 odom_vel_x +float64 odom_vel_y +float64 odom_vel_tw + +# Step 1 info (LIMIT polygon collision check before steering validation) +string step1_active_sub_polygon # sub-polygon name active during step 1 +int32 step1_points_inside # collision points found inside the active polygon +int32 step1_min_points # threshold for triggering action +bool step1_shape_set # whether polygon shape was set +float64 step1_linear_limit # linear limit of the active sub-polygon (baselink) +float64 step1_angular_limit # angular limit of the active sub-polygon +uint8 step1_action_type # action type after step 1 (0=DO_NOTHING,1=STOP,2=SLOWDOWN,3=APPROACH,4=LIMIT) +float64 step1_req_vel_x # velocity x after step 1 +float64 step1_req_vel_y # velocity y after step 1 +float64 step1_req_vel_tw # velocity tw after step 1 + +# Source filtering diagnostics +string[] step1_configured_sources # source names this polygon is configured to check +int32 step1_configured_source_pts # total points from configured sources (before polygon test) +int32 step1_all_source_pts # total points across ALL sources in the map +int32 step1_polygon_vertex_count # number of vertices in the active polygon shape + +# Bounding box diagnostics — compare polygon bounds vs collision point bounds +float64 step1_poly_min_x +float64 step1_poly_max_x +float64 step1_poly_min_y +float64 step1_poly_max_y +float64 step1_pts_min_x # min x of all collision points from configured sources +float64 step1_pts_max_x +float64 step1_pts_min_y +float64 step1_pts_max_y +int32 step1_pts_in_bbox # points within the polygon's bounding box (but not necessarily inside polygon) + +# Step 2 decision path +bool crosses_zero # direction reversal detected +bool both_below_threshold # both speeds below low_speed_threshold +bool same_bucket # target angle in same bucket as current +string current_field_name # field matching current velocity ("" if none) +string valid_field_name # collision-free neighbour field ("" if N/A) +string next_field_name # one-step-faster field checked in same-bucket case + +# Collision info +int32 next_field_collision_pts # points inside next field (same-bucket case) +int32 neighbour_collision_pts # points inside neighbour start field (diff-bucket case) + +# Modification result +bool modified # whether velocity was modified by step 2 +float64 result_vel_x # final output velocity x +float64 result_vel_y # final output velocity y +float64 result_vel_tw # final output velocity tw + +# Limits applied +float64 speed_limit_applied # baselink speed limit applied (0 if none) +float64 steering_angle_limit # steering angle limit applied (NaN if none) + +# Polygon name +string polygon_name From 486c2a5b4bc5541a060466eb5097555315a1846a Mon Sep 17 00:00:00 2001 From: Johannes Plapp Date: Thu, 12 Mar 2026 14:46:03 +0100 Subject: [PATCH 03/35] . --- nav2_collision_monitor/README.md | 23 +- .../velocity_polygon.hpp | 10 + .../src/collision_monitor_node.cpp | 14 + .../src/velocity_polygon.cpp | 115 ++++++-- .../test/velocity_polygons_test.cpp | 256 +++++++++++++++++- 5 files changed, 384 insertions(+), 34 deletions(-) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 043467bbf2e..ba33a3b463c 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -159,4 +159,25 @@ else: → done -After some iterations, the current speed will be in the valid field → AMR will be allowed to steer, as in 4b, the current speed will not be above max valid speed \ No newline at end of file +After some iterations, the current speed will be in the valid field → AMR will be allowed to steer, as in 4b, 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. + +#### 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. \ No newline at end of file diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp index 4ef1daf72b3..a405740f53f 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp @@ -92,6 +92,16 @@ class VelocityPolygon : public Polygon const std::unordered_map> & 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 diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 218b75a14fc..d74f75a3736 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -569,6 +569,20 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: } } + // 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(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); diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 370d904102a..1f583ede732 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -723,32 +723,29 @@ bool VelocityPolygon::validateSteering( } } - // Check if fastest field is collision-free → done + // Determine valid field: fastest collision-free field in neighbor bucket int start_pts = getPointsInsideSubPolygon( *neighbour_fields[start_idx], collision_points_map); debug_msg.neighbour_collision_pts = start_pts; if (start_pts < min_points_) { - debug_msg.modified = false; - debug_msg.result_vel_x = result_vel.x; - debug_msg.result_vel_y = result_vel.y; - debug_msg.result_vel_tw = result_vel.tw; - steering_debug_pub_->publish(debug_msg); - return false; - } - - // Walk down speed fields until a collision-free one is found - for (int i = start_idx; i >= 0; i--) { - if (getPointsInsideSubPolygon(*neighbour_fields[i], collision_points_map) < min_points_) { - valid_field = neighbour_fields[i]; - break; + // Fastest field is collision-free — use it as valid field. + // Speed must still be limited to this field's max to prevent field exceedance + // when entering a bucket with lower max speed (e.g. straight → turned). + valid_field = neighbour_fields[start_idx]; + } else { + // Walk down speed fields until a collision-free one is found + for (int i = start_idx; i >= 0; i--) { + if (getPointsInsideSubPolygon(*neighbour_fields[i], collision_points_map) < min_points_) { + valid_field = neighbour_fields[i]; + break; + } } - } - - if (valid_field == nullptr) { - // All fields in collision — use the slowest field in the target direction - // (allowed even if in collision) - valid_field = neighbour_fields[0]; // sorted ascending, index 0 is slowest + if (valid_field == nullptr) { + // All fields in collision — use the slowest field in the target direction + // (allowed even if in collision) + valid_field = neighbour_fields[0]; // sorted ascending, index 0 is slowest + } } debug_msg.valid_field_name = valid_field->velocity_polygon_name_; @@ -762,6 +759,7 @@ bool VelocityPolygon::validateSteering( if (std::abs(result_vel.x) > std::abs(valid_max_baselink)) { debug_msg.speed_limit_applied = valid_max_baselink; result_vel.x = valid_max_baselink; + modified = true; } // 4b. Only limit steering angle if current speed is larger than max valid speed @@ -776,18 +774,85 @@ bool VelocityPolygon::validateSteering( } debug_msg.steering_angle_limit = limited_sa; result_vel.tw = steeringAngleToTw(result_vel.x, limited_sa); + modified = true; } - robot_action.req_vel = result_vel; - robot_action.polygon_name = polygon_name_; - robot_action.action_type = LIMIT; + if (modified) { + robot_action.req_vel = result_vel; + robot_action.polygon_name = polygon_name_; + robot_action.action_type = LIMIT; + } - debug_msg.modified = true; + debug_msg.modified = modified; debug_msg.result_vel_x = result_vel.x; debug_msg.result_vel_y = result_vel.y; debug_msg.result_vel_tw = result_vel.tw; steering_debug_pub_->publish(debug_msg); - return true; + return modified; +} + +bool VelocityPolygon::clampToMaxField( + const Velocity & odom_vel, Action & robot_action) +{ + // Only applies to steering-angle-based velocity polygons + if (sub_polygons_.empty() || !sub_polygons_[0].use_steering_angle_) { + return false; + } + + // Physical steering angle from odometry + double physical_sa = computeSteeringAngle(odom_vel); + + // Direction from commanded velocity + double cmd_x = robot_action.req_vel.x; + bool forward = cmd_x >= 0; + + // Find all fields at the physical steering angle for the commanded direction + auto fields = findFieldsForAngle(physical_sa, forward); + + if (fields.empty()) { + // No fields at this angle — if velocity is non-zero, zero it + if (std::abs(cmd_x) > 1e-6) { + RCLCPP_INFO( + logger_, + "[%s] clampToMaxField: no fields at physical_sa=%.3f, zeroing velocity", + polygon_name_.c_str(), physical_sa); + robot_action.req_vel.x = 0.0; + robot_action.req_vel.y = 0.0; + robot_action.req_vel.tw = 0.0; + robot_action.polygon_name = polygon_name_; + robot_action.action_type = LIMIT; + return true; + } + return false; + } + + // Fastest field is the last one (sorted slowest first) + const SubPolygonParameter * fastest = fields.back(); + + // Max steering wheel speed for this field + double max_sw = forward ? fastest->linear_max_ : fastest->linear_min_; + + // Compute commanded steering angle and steering wheel speed + double cmd_sa = computeSteeringAngle(robot_action.req_vel); + double cmd_sw = baselinkToSteeringSpeed(robot_action.req_vel.x, robot_action.req_vel.tw); + + // Check if commanded sw speed exceeds max + if (std::abs(cmd_sw) > std::abs(max_sw)) { + double clamped_baselink = steeringToBaselinkSpeed(max_sw, cmd_sa); + RCLCPP_INFO( + logger_, + "[%s] clampToMaxField: cmd_sw=%.3f exceeds max_sw=%.3f at physical_sa=%.3f, " + "clamping vel.x from %.3f to %.3f", + polygon_name_.c_str(), cmd_sw, max_sw, physical_sa, + robot_action.req_vel.x, clamped_baselink); + robot_action.req_vel.x = clamped_baselink; + robot_action.req_vel.tw = steeringAngleToTw(clamped_baselink, cmd_sa); + robot_action.polygon_name = polygon_name_; + robot_action.action_type = LIMIT; + return true; + } + + return false; } } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index 2b7a477170a..765c2357215 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -183,6 +183,13 @@ class VelocityPolygonWrapper : public nav2_collision_monitor::VelocityPolygon { return getPointsInsideSubPolygon(sub_polygon, collision_points_map); } + + bool callClampToMaxField( + const nav2_collision_monitor::Velocity & odom_vel, + nav2_collision_monitor::Action & robot_action) + { + return clampToMaxField(odom_vel, robot_action); + } }; // VelocityPolygonWrapper class Tester : public ::testing::Test @@ -1127,14 +1134,8 @@ TEST_F(Tester, testValidateSteeringDifferentBucketAllCollision) nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; bool modified = velocity_polygon_->validateSteering(cmd_vel, odom_vel, collision_map, action); - EXPECT_TRUE(modified); - // All fields in collision → use slowest field (allowed even if in collision), LIMIT not STOP - EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); - // Speed should be limited to slowest field's linear_max (0.5) converted to baselink - // At the neighbour bucket boundary (0.1 rad) - double neighbour_angle = 0.1; // straight bucket boundary toward left - double expected_max = 0.5 * std::cos(neighbour_angle); - EXPECT_LE(std::abs(action.req_vel.x), expected_max + 1e-3); + // Speed (0.3) is already within the slowest field's max (0.5), so nothing to change + EXPECT_FALSE(modified); } TEST_F(Tester, testValidateSteeringDifferentBucketDecelerationToValidField) @@ -1230,6 +1231,245 @@ TEST_F(Tester, testValidateSteeringNotApplicableToNonSteering) EXPECT_FALSE(modified); // Should not apply to theta-based polygons } +// ==================== validateSteering different-bucket exceedance fix tests ==================== + +TEST_F(Tester, testValidateSteeringDifferentBucketExceedsFastestField) +{ + // straight: slow[0,0.5] + fast[0.5,1.0] sa[-0.1,0.1] + // left: slow[0,0.5] only sa[0.1,0.5] + // When steering from straight into left at speed 0.7, which exceeds left's max (0.5), + // speed should be clamped even though left is collision-free. + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, + {"straight_slow", "straight_fast", "left_slow"}); + addSteeringAngleSubPolygon("straight_slow", 0.0, 0.5, -0.1, 0.1, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("straight_fast", 0.5, 1.0, -0.1, 0.1, STEERING_POLYGON_FAST_STR); + addSteeringAngleSubPolygon("left_slow", 0.0, 0.5, 0.1, 0.5, STEERING_POLYGON_SLOW_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity vel{0.7, 0.0, 0.0}; + velocity_polygon_->updatePolygon(vel); + + // Current: straight at 0.7 (fast field), Target: left at 0.7 + nav2_collision_monitor::Velocity odom_vel{0.7, 0.0, 0.0}; + double target_sa = 0.3; + double target_tw = std::tan(target_sa) * 0.7 / WHEELBASE; + nav2_collision_monitor::Velocity cmd_vel{0.7, 0.0, target_tw}; + + std::unordered_map> collision_map; + collision_map["source"] = {}; // no collisions + + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->validateSteering(cmd_vel, odom_vel, collision_map, action); + EXPECT_TRUE(modified); + EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); + // Speed should be clamped to left_slow's max (0.5) at neighbour boundary (0.1 rad) + double neighbour_angle = 0.1; + double expected_max = 0.5 * std::cos(neighbour_angle); + EXPECT_LE(std::abs(action.req_vel.x), expected_max + 1e-3); +} + +// ==================== clampToMaxField tests ==================== + +TEST_F(Tester, testClampToMaxFieldNoClamping) +{ + // slow[0,0.5] + fast[0.5,1.0] sa[-0.5,0.5] + // phys_sa=0, cmd=0.8 → within 1.0 max, no clamping + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"slow", "fast"}); + addSteeringAngleSubPolygon("slow", 0.0, 0.5, -0.5, 0.5, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("fast", 0.5, 1.0, -0.5, 0.5, STEERING_POLYGON_FAST_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity odom_vel{0.8, 0.0, 0.0}; // phys_sa=0 + nav2_collision_monitor::Velocity cmd_vel{0.8, 0.0, 0.0}; + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->callClampToMaxField(odom_vel, action); + EXPECT_FALSE(modified); +} + +TEST_F(Tester, testClampToMaxFieldExceedsMax) +{ + // slow[0,0.5] only sa[-0.5,0.5] + // phys_sa=0, cmd=0.8 → exceeds 0.5, should clamp + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"slow"}); + addSteeringAngleSubPolygon("slow", 0.0, 0.5, -0.5, 0.5, STEERING_POLYGON_SLOW_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity odom_vel{0.3, 0.0, 0.0}; // phys_sa=0 + nav2_collision_monitor::Velocity cmd_vel{0.8, 0.0, 0.0}; + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->callClampToMaxField(odom_vel, action); + EXPECT_TRUE(modified); + EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); + // Clamped to 0.5 at sa=0 → baselink = 0.5 * cos(0) = 0.5 + EXPECT_LE(std::abs(action.req_vel.x), 0.5 + 1e-6); +} + +TEST_F(Tester, testClampToMaxFieldTurnedBucketLower) +{ + // straight: slow[0,0.5] + fast[0.5,1.0] sa[-0.1,0.1] + // turned: slow[0,0.5] only sa[0.3,0.6] + // phys_sa=0.4 (turned bucket), cmd=0.8 → exceeds turned max (0.5), clamp + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, + {"straight_slow", "straight_fast", "turned_slow"}); + addSteeringAngleSubPolygon("straight_slow", 0.0, 0.5, -0.1, 0.1, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("straight_fast", 0.5, 1.0, -0.1, 0.1, STEERING_POLYGON_FAST_STR); + addSteeringAngleSubPolygon("turned_slow", 0.0, 0.5, 0.3, 0.6, STEERING_POLYGON_SLOW_STR); + createSteeringVelocityPolygon("limit"); + + double phys_sa = 0.4; + double phys_tw = std::tan(phys_sa) * 0.5 / WHEELBASE; + nav2_collision_monitor::Velocity odom_vel{0.5, 0.0, phys_tw}; // phys_sa=0.4 + + // cmd targets some steering angle at 0.8 speed + double cmd_sa = 0.2; + double cmd_tw = std::tan(cmd_sa) * 0.8 / WHEELBASE; + nav2_collision_monitor::Velocity cmd_vel{0.8, 0.0, cmd_tw}; + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->callClampToMaxField(odom_vel, action); + EXPECT_TRUE(modified); + EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); + // Max sw speed at physical angle (0.4) is 0.5, clamped baselink = 0.5 * cos(cmd_sa) + double expected_max_baselink = 0.5 * std::cos(cmd_sa); + EXPECT_LE(std::abs(action.req_vel.x), expected_max_baselink + 1e-3); +} + +TEST_F(Tester, testClampToMaxFieldRule2NotYetInBucket) +{ + // Rule 2: turned→straight transition, still physically turned + // turned[0.3,0.6]: slow[0,0.5]; straight[-0.1,0.1]: slow[0,0.5]+fast[0.5,1.0] + // phys_sa=0.4 (turned), cmd targets straight at 0.8 → clamp to turned max (0.5) + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, + {"turned_slow", "straight_slow", "straight_fast"}); + addSteeringAngleSubPolygon("turned_slow", 0.0, 0.5, 0.3, 0.6, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("straight_slow", 0.0, 0.5, -0.1, 0.1, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("straight_fast", 0.5, 1.0, -0.1, 0.1, STEERING_POLYGON_FAST_STR); + createSteeringVelocityPolygon("limit"); + + double phys_sa = 0.4; + double phys_tw = std::tan(phys_sa) * 0.3 / WHEELBASE; + nav2_collision_monitor::Velocity odom_vel{0.3, 0.0, phys_tw}; // phys in turned bucket + + // Cmd targets straight at 0.8 + nav2_collision_monitor::Velocity cmd_vel{0.8, 0.0, 0.0}; + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->callClampToMaxField(odom_vel, action); + EXPECT_TRUE(modified); + EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); + // Max at phys_sa=0.4 is turned_slow max = 0.5 + // cmd_sa=0, so clamped baselink = 0.5 * cos(0) = 0.5 + EXPECT_LE(std::abs(action.req_vel.x), 0.5 + 1e-3); +} + +TEST_F(Tester, testClampToMaxFieldRule2Arrived) +{ + // Same setup as above but phys_sa=0.0 (arrived in straight bucket) + // cmd=0.8 → within straight fast max (1.0), no clamping + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, + {"turned_slow", "straight_slow", "straight_fast"}); + addSteeringAngleSubPolygon("turned_slow", 0.0, 0.5, 0.3, 0.6, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("straight_slow", 0.0, 0.5, -0.1, 0.1, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("straight_fast", 0.5, 1.0, -0.1, 0.1, STEERING_POLYGON_FAST_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity odom_vel{0.8, 0.0, 0.0}; // phys_sa=0, in straight bucket + + nav2_collision_monitor::Velocity cmd_vel{0.8, 0.0, 0.0}; + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->callClampToMaxField(odom_vel, action); + EXPECT_FALSE(modified); // Within fast field max, no clamping needed +} + +TEST_F(Tester, testClampToMaxFieldBackward) +{ + // backward[-0.5,0] sa[-0.5,0.5] + // phys_sa=0, cmd.x=-0.8 → exceeds -0.5, should clamp + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"backward"}); + addSteeringAngleSubPolygon("backward", -0.5, 0.0, -0.5, 0.5, STEERING_POLYGON_SLOW_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity odom_vel{-0.3, 0.0, 0.0}; // phys_sa=0 + nav2_collision_monitor::Velocity cmd_vel{-0.8, 0.0, 0.0}; + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->callClampToMaxField(odom_vel, action); + EXPECT_TRUE(modified); + EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); + // Clamped to -0.5 at sa=0 → baselink = -0.5 * cos(0) = -0.5 + EXPECT_GE(action.req_vel.x, -0.5 - 1e-6); +} + +TEST_F(Tester, testClampToMaxFieldNoFieldsZeros) +{ + // Fields only at sa[0.3,0.6] + // phys_sa=0.0 → no fields found, velocity should be zeroed + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"turned_only"}); + addSteeringAngleSubPolygon("turned_only", 0.0, 0.5, 0.3, 0.6, STEERING_POLYGON_SLOW_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity odom_vel{0.3, 0.0, 0.0}; // phys_sa=0 + nav2_collision_monitor::Velocity cmd_vel{0.3, 0.0, 0.0}; + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->callClampToMaxField(odom_vel, action); + EXPECT_TRUE(modified); + EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); + EXPECT_NEAR(action.req_vel.x, 0.0, 1e-6); + EXPECT_NEAR(action.req_vel.tw, 0.0, 1e-6); +} + +TEST_F(Tester, testClampToMaxFieldNonSteeringSkipped) +{ + // Create a normal theta-based velocity polygon (not steering angle) + createVelocityPolygon("stop", IS_NOT_HOLONOMIC); + + nav2_collision_monitor::Velocity odom_vel{0.3, 0.0, 0.0}; + nav2_collision_monitor::Velocity cmd_vel{0.3, 0.0, 0.0}; + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->callClampToMaxField(odom_vel, action); + EXPECT_FALSE(modified); // Should not apply to theta-based polygons +} + +TEST_F(Tester, testClampToMaxFieldPreservesDirection) +{ + // slow[0,0.5] sa[-0.5,0.5] + // phys_sa=0, cmd x=0.8 tw=0.3 → speed clamped, tw adjusted to same steering angle + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"slow"}); + addSteeringAngleSubPolygon("slow", 0.0, 0.5, -0.5, 0.5, STEERING_POLYGON_SLOW_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity odom_vel{0.3, 0.0, 0.0}; // phys_sa=0 + double cmd_sa_input = 0.2; + double cmd_tw_input = std::tan(cmd_sa_input) * 0.8 / WHEELBASE; + nav2_collision_monitor::Velocity cmd_vel{0.8, 0.0, cmd_tw_input}; + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->callClampToMaxField(odom_vel, action); + EXPECT_TRUE(modified); + EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); + // Speed clamped, steering angle should be preserved + double clamped_sa = velocity_polygon_->callComputeSteeringAngle(action.req_vel); + EXPECT_NEAR(clamped_sa, cmd_sa_input, 0.05); + // Speed should be limited + EXPECT_LE(std::abs(action.req_vel.x), 0.5 + 1e-3); +} + int main(int argc, char ** argv) { From 588ac6dbeaa2e12f0ab919d547029d0b102b8117 Mon Sep 17 00:00:00 2001 From: Johannes Plapp Date: Tue, 17 Mar 2026 12:00:26 +0100 Subject: [PATCH 04/35] logs --- .../include/nav2_collision_monitor/source.hpp | 2 ++ .../src/collision_monitor_node.cpp | 15 +++++++++++++++ nav2_collision_monitor/src/costmap.cpp | 3 +++ nav2_collision_monitor/src/pointcloud.cpp | 7 +++++++ nav2_collision_monitor/src/polygon_source.cpp | 3 +++ nav2_collision_monitor/src/range.cpp | 11 +++++++++-- nav2_collision_monitor/src/scan.cpp | 13 +++++++++++++ nav2_collision_monitor/src/source.cpp | 2 ++ 8 files changed, 54 insertions(+), 2 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 10bcdb094b0..dac95779963 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/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_; diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index d74f75a3736..248df8c62ab 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -453,6 +453,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; @@ -461,6 +465,17 @@ 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()); + } + } else { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 2000, + "[%s]: Source is disabled — skipping getData(), 0 points contributed", + source->getSourceName().c_str()); } if (collision_points_marker_pub_->get_subscription_count() > 0) { diff --git a/nav2_collision_monitor/src/costmap.cpp b/nav2_collision_monitor/src/costmap.cpp index befff7e4ca0..43f432a667b 100644 --- a/nav2_collision_monitor/src/costmap.cpp +++ b/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; } diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 58c0dafa3a1..4b2073cee23 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -109,6 +109,9 @@ bool PointCloud::getData( // Ignore data from the source if it is not being published yet or // not published for a long time if (data_ == nullptr) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 2000, + "[%s]: No pointcloud data received yet (data_ is null)", source_name_.c_str()); return false; } if (!sourceValid(data_->header.stamp, curr_time)) { @@ -117,6 +120,10 @@ bool PointCloud::getData( tf2::Transform tf_transform; if (!getTransform(curr_time, data_->header, tf_transform)) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 2000, + "[%s]: TF lookup failed for pointcloud (frame=%s)", + source_name_.c_str(), data_->header.frame_id.c_str()); return false; } diff --git a/nav2_collision_monitor/src/polygon_source.cpp b/nav2_collision_monitor/src/polygon_source.cpp index eb29ed4c447..33fbcf2fcfc 100644 --- a/nav2_collision_monitor/src/polygon_source.cpp +++ b/nav2_collision_monitor/src/polygon_source.cpp @@ -72,6 +72,9 @@ bool PolygonSource::getData( // Ignore data from the source if it is not being published yet or // not published for a long time if (data_.empty()) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 2000, + "[%s]: No polygon source data received yet (data_ is empty)", source_name_.c_str()); return false; } diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 6df4d27c513..e078d339aaf 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -74,6 +74,9 @@ bool Range::getData( // Ignore data from the source if it is not being published yet or // not being published for a long time if (data_ == nullptr) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 2000, + "[%s]: No range data received yet (data_ is null)", source_name_.c_str()); return false; } if (!sourceValid(data_->header.stamp, curr_time)) { @@ -82,8 +85,8 @@ bool Range::getData( // Ignore data, if its range is out of scope of range sensor abilities if (data_->range < data_->min_range || data_->range > data_->max_range) { - RCLCPP_DEBUG( - logger_, + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 2000, "[%s]: Data range %fm is out of {%f..%f} sensor span. Ignoring...", source_name_.c_str(), data_->range, data_->min_range, data_->max_range); return false; @@ -91,6 +94,10 @@ bool Range::getData( tf2::Transform tf_transform; if (!getTransform(curr_time, data_->header, tf_transform)) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 2000, + "[%s]: TF lookup failed for range (frame=%s)", + source_name_.c_str(), data_->header.frame_id.c_str()); return false; } diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index ce5fa7c89ec..b4c130c9ad8 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -73,14 +73,27 @@ bool Scan::getData( // Ignore data from the source if it is not being published yet or // not being published for a long time if (data_ == nullptr) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 2000, + "[%s]: No scan data received yet (data_ is null)", source_name_.c_str()); return false; } if (!sourceValid(data_->header.stamp, curr_time)) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 2000, + "[%s]: Scan data is stale (stamp=%.3f, now=%.3f, timeout=%.3fs)", + source_name_.c_str(), + rclcpp::Time(data_->header.stamp).seconds(), curr_time.seconds(), + source_timeout_.seconds()); return false; } tf2::Transform tf_transform; if (!getTransform(curr_time, data_->header, tf_transform)) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 2000, + "[%s]: TF lookup failed for scan (frame=%s)", + source_name_.c_str(), data_->header.frame_id.c_str()); return false; } diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index ec92d007747..be5cdb01bc0 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -57,6 +57,8 @@ bool Source::configure() { auto node = node_.lock(); + clock_ = node->get_clock(); + // Add callback for dynamic parameters post_set_params_handler_ = node->add_post_set_parameters_callback( std::bind( From 47783a5523333aaaa976c951ef99d85887cecb45 Mon Sep 17 00:00:00 2001 From: Johannes Plapp Date: Tue, 17 Mar 2026 14:13:23 +0100 Subject: [PATCH 05/35] . --- nav2_collision_monitor/src/velocity_polygon.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 1f583ede732..f910f90b03a 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -604,8 +604,14 @@ bool VelocityPolygon::validateSteering( // Speed does NOT cross zero // 1. Both abs(target) and abs(current) below threshold → done - bool both_below = std::abs(target_speed) < low_speed_threshold_ && - std::abs(current_speed) < low_speed_threshold_; + // Use steering wheel speed (accounts for angular velocity) instead of baselink linear speed + double target_sw_speed = baselinkToSteeringSpeed(cmd_vel_in.x, cmd_vel_in.tw); + double current_sw_speed = baselinkToSteeringSpeed(odom_vel.x, odom_vel.tw); + debug_msg.target_sw_speed = target_sw_speed; + debug_msg.current_sw_speed = current_sw_speed; + + bool both_below = std::abs(target_sw_speed) < low_speed_threshold_ && + std::abs(current_sw_speed) < low_speed_threshold_; debug_msg.both_below_threshold = both_below; if (both_below) { debug_msg.modified = false; @@ -616,11 +622,6 @@ bool VelocityPolygon::validateSteering( return false; } - double target_sw_speed = baselinkToSteeringSpeed(cmd_vel_in.x, cmd_vel_in.tw); - double current_sw_speed = baselinkToSteeringSpeed(odom_vel.x, odom_vel.tw); - debug_msg.target_sw_speed = target_sw_speed; - debug_msg.current_sw_speed = current_sw_speed; - const SubPolygonParameter * current_field = findField(current_sw_speed, current_sa); debug_msg.current_field_name = current_field ? current_field->velocity_polygon_name_ : ""; From 8e226a9da82f9f7d8ce084e7207bf07fda764067 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Tue, 17 Mar 2026 16:27:40 +0100 Subject: [PATCH 06/35] Fixed Logic for correct field determination. --- .../src/velocity_polygon.cpp | 51 +++++++++---------- 1 file changed, 23 insertions(+), 28 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index f910f90b03a..24c5b9782e2 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -633,36 +633,31 @@ bool VelocityPolygon::validateSteering( debug_msg.same_bucket = same_bucket; if (same_bucket) { - // If result velocity falls into some faster field (same bucket), check the - // one-step-faster field for collision. If in collision, limit to current field. - double result_sw_speed = baselinkToSteeringSpeed(result_vel.x, result_vel.tw); - const SubPolygonParameter * result_field = findField(result_sw_speed, target_steering_angle); - if (result_field != nullptr && result_field != current_field && - std::abs(result_sw_speed) > std::abs(current_sw_speed)) - { - bool forward = current_sw_speed >= 0; - auto fields_at_angle = findFieldsForAngle(target_steering_angle, forward); - for (size_t i = 0; i < fields_at_angle.size(); i++) { - if (fields_at_angle[i] == current_field && i + 1 < fields_at_angle.size()) { - const SubPolygonParameter * next_field = fields_at_angle[i + 1]; - debug_msg.next_field_name = next_field->velocity_polygon_name_; - int pts = getPointsInsideSubPolygon(*next_field, collision_points_map); - debug_msg.next_field_collision_pts = pts; - if (pts >= min_points_) { - // Forward: limit to linear_max (upper bound) - // Backward: limit to linear_min (lower bound, more negative = faster) - double limit_sw = (current_sw_speed >= 0) ? - current_field->linear_max_ : current_field->linear_min_; - double max_baselink = steeringToBaselinkSpeed(limit_sw, current_sa); - if (std::abs(result_vel.x) > std::abs(max_baselink)) { - debug_msg.speed_limit_applied = max_baselink; - result_vel.x = max_baselink; - result_vel.tw = steeringAngleToTw(result_vel.x, target_steering_angle); - modified = true; - } + // Proactively check the next-faster field at the current steering angle. + // If that field has obstacles, limit speed to the current field's max + // so the robot can never cross into the e-stop zone. + bool forward = current_sw_speed >= 0; + auto fields_at_angle = findFieldsForAngle(target_steering_angle, forward); + for (size_t i = 0; i < fields_at_angle.size(); i++) { + if (fields_at_angle[i] == current_field && i + 1 < fields_at_angle.size()) { + const SubPolygonParameter * next_field = fields_at_angle[i + 1]; + debug_msg.next_field_name = next_field->velocity_polygon_name_; + int pts = getPointsInsideSubPolygon(*next_field, collision_points_map); + debug_msg.next_field_collision_pts = pts; + if (pts >= min_points_) { + // Forward: limit to linear_max (upper bound) + // Backward: limit to linear_min (lower bound, more negative = faster) + double limit_sw = (current_sw_speed >= 0) ? + current_field->linear_max_ : current_field->linear_min_; + double max_baselink = steeringToBaselinkSpeed(limit_sw, current_sa); + if (std::abs(result_vel.x) > std::abs(max_baselink)) { + debug_msg.speed_limit_applied = max_baselink; + result_vel.x = max_baselink; + result_vel.tw = steeringAngleToTw(result_vel.x, target_steering_angle); + modified = true; } - break; } + break; } } // Same bucket → done From 221058b26ab5fc169faf70ce9ff11582312de7e3 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Tue, 17 Mar 2026 17:04:13 +0100 Subject: [PATCH 07/35] . --- .../src/velocity_polygon.cpp | 52 +++++++++++-------- 1 file changed, 29 insertions(+), 23 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 24c5b9782e2..786e598eef9 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -633,31 +633,37 @@ bool VelocityPolygon::validateSteering( debug_msg.same_bucket = same_bucket; if (same_bucket) { - // Proactively check the next-faster field at the current steering angle. - // If that field has obstacles, limit speed to the current field's max - // so the robot can never cross into the e-stop zone. - bool forward = current_sw_speed >= 0; - auto fields_at_angle = findFieldsForAngle(target_steering_angle, forward); - for (size_t i = 0; i < fields_at_angle.size(); i++) { - if (fields_at_angle[i] == current_field && i + 1 < fields_at_angle.size()) { - const SubPolygonParameter * next_field = fields_at_angle[i + 1]; - debug_msg.next_field_name = next_field->velocity_polygon_name_; - int pts = getPointsInsideSubPolygon(*next_field, collision_points_map); - debug_msg.next_field_collision_pts = pts; - if (pts >= min_points_) { - // Forward: limit to linear_max (upper bound) - // Backward: limit to linear_min (lower bound, more negative = faster) - double limit_sw = (current_sw_speed >= 0) ? - current_field->linear_max_ : current_field->linear_min_; - double max_baselink = steeringToBaselinkSpeed(limit_sw, current_sa); - if (std::abs(result_vel.x) > std::abs(max_baselink)) { - debug_msg.speed_limit_applied = max_baselink; - result_vel.x = max_baselink; - result_vel.tw = steeringAngleToTw(result_vel.x, target_steering_angle); - modified = true; + // If result velocity falls into some faster field (same bucket), check the + // one-step-faster field for collision. If in collision, limit to current field. + double result_sw_speed = baselinkToSteeringSpeed(result_vel.x, result_vel.tw); + const SubPolygonParameter * result_field = findField(result_sw_speed, target_steering_angle); + if (result_field != nullptr && result_field != current_field && + std::abs(result_sw_speed) > std::abs(current_sw_speed)) + { + bool forward = current_sw_speed >= 0; + auto fields_at_angle = findFieldsForAngle(target_steering_angle, forward); + for (size_t i = 0; i < fields_at_angle.size(); i++) { + if (fields_at_angle[i] == current_field && i + 1 < fields_at_angle.size()) { + const SubPolygonParameter * next_field = fields_at_angle[i + 1]; + debug_msg.next_field_name = next_field->velocity_polygon_name_; + int pts = getPointsInsideSubPolygon(*next_field, collision_points_map); + debug_msg.next_field_collision_pts = pts; + if (pts >= min_points_) { + // Limit steering wheel speed to current field boundary. + // Decompose directly into x and tw to avoid tan(π/2) singularity. + double limit_sw = (current_sw_speed >= 0) ? + current_field->linear_max_ : current_field->linear_min_; + if (std::abs(result_sw_speed) > std::abs(limit_sw)) { + double limited_x = limit_sw * std::cos(target_steering_angle); + double limited_tw = limit_sw * std::sin(target_steering_angle) / wheelbase_; + debug_msg.speed_limit_applied = limited_x; + result_vel.x = limited_x; + result_vel.tw = limited_tw; + modified = true; + } } + break; } - break; } } // Same bucket → done From 384d4a66abd4d5f4838b3952800a64caebe0c872 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Tue, 17 Mar 2026 17:11:45 +0100 Subject: [PATCH 08/35] . --- .../src/velocity_polygon.cpp | 17 ++++++ .../test/velocity_polygons_test.cpp | 53 +++++++++++++++++++ 2 files changed, 70 insertions(+) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 786e598eef9..b600699b0e6 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -681,6 +681,23 @@ bool VelocityPolygon::validateSteering( } // 3. Different bucket — find neighbouring bucket (one step in steering direction) + if (current_field == nullptr) { + RCLCPP_WARN( + logger_, + "[%s] validateSteering: current_field is null — no field matches current velocity. " + "odom_vel=(%.3f, %.3f, %.3f), current_sw_speed=%.3f, current_sa=%.3f, " + "cmd_vel=(%.3f, %.3f, %.3f), target_sw_speed=%.3f, target_sa=%.3f. " + "Check that velocity polygon field ranges cover all reachable velocities.", + polygon_name_.c_str(), + odom_vel.x, odom_vel.y, odom_vel.tw, current_sw_speed, current_sa, + cmd_vel_in.x, cmd_vel_in.y, cmd_vel_in.tw, target_sw_speed, target_steering_angle); + debug_msg.modified = false; + debug_msg.result_vel_x = result_vel.x; + debug_msg.result_vel_y = result_vel.y; + debug_msg.result_vel_tw = result_vel.tw; + steering_debug_pub_->publish(debug_msg); + return false; + } double neighbour_angle; if (target_steering_angle > current_sa) { diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index 765c2357215..548ce9062f7 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -1471,6 +1471,59 @@ TEST_F(Tester, testClampToMaxFieldPreservesDirection) } +TEST_F(Tester, testValidateSteeringSameBucket90DegLimitsTw) +{ + // Two speed tiers at 90° steering angle range. + // At 90° steering the robot is doing pure rotation (x≈0, tw carries all speed). + // The old code compared result_vel.x against max_baselink (both ≈0 at 90°) + // and never triggered a limit. The fix compares steering wheel speeds and + // decomposes via sin/cos so that tw is correctly limited. + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, {"slow_turn", "fast_turn"}); + // steering_angle range [1.0, 1.571] covers 90° (π/2 ≈ 1.5708) + addSteeringAngleSubPolygon("slow_turn", 0.0, 0.3, 1.0, 1.571, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("fast_turn", 0.3, 1.0, 1.0, 1.571, STEERING_POLYGON_FAST_STR); + createSteeringVelocityPolygon("limit"); + + // updatePolygon with a velocity that maps to the slow field at ~90° + // Pure rotation: x=0, tw=0.2 → sw_speed = hypot(0, 1.0*0.2) = 0.2 (in slow field) + nav2_collision_monitor::Velocity vel{0.0, 0.0, 0.2}; + velocity_polygon_->updatePolygon(vel); + + // cmd_vel: pure rotation, steering angle = π/2 + // x=0, tw=0.5 → sw_speed = hypot(0, 1.0*0.5) = 0.5 (in fast field) + nav2_collision_monitor::Velocity cmd_vel{0.0, 0.0, 0.5}; + // odom: currently in slow field + nav2_collision_monitor::Velocity odom_vel{0.0, 0.0, 0.15}; + + // Place collision points inside the fast polygon + std::unordered_map> collision_map; + collision_map["source"] = { + {1.0, 0.0}, + {1.2, 0.1}, + }; + + // robot_action with result velocity in the fast field (sw_speed = 0.5) + nav2_collision_monitor::Velocity action_vel{0.0, 0.0, 0.5}; + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, action_vel, ""}; + + bool modified = velocity_polygon_->validateSteering(cmd_vel, odom_vel, collision_map, action); + EXPECT_TRUE(modified); + EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); + + // At 90° steering, x should be ~0 and tw should be limited + EXPECT_NEAR(action.req_vel.x, 0.0, 1e-6); + // tw should be limited to slow field's linear_max (0.3) decomposed at 90°: + // limited_tw = limit_sw * sin(π/2) / wheelbase = 0.3 * 1.0 / 1.0 = 0.3 + EXPECT_LE(std::abs(action.req_vel.tw), 0.3 + 1e-6); + EXPECT_GT(std::abs(action.req_vel.tw), 0.0); // tw should not be zeroed + + // Verify the steering wheel speed of the result doesn't exceed the slow field max + double result_sw = velocity_polygon_->callBaselinkToSteeringSpeed( + action.req_vel.x, action.req_vel.tw); + EXPECT_LE(std::abs(result_sw), 0.3 + 1e-6); +} + int main(int argc, char ** argv) { // Initialize the system From 79c4ed1fa8d8f590d9b6e2314e006560324d1041 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Thu, 19 Mar 2026 14:12:24 +0100 Subject: [PATCH 09/35] Fixed issue in collision_monitor for finding next field. --- .../src/velocity_polygon.cpp | 72 +++++++++++-------- 1 file changed, 42 insertions(+), 30 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index b600699b0e6..63e9fc95702 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -633,39 +633,51 @@ bool VelocityPolygon::validateSteering( debug_msg.same_bucket = same_bucket; if (same_bucket) { - // If result velocity falls into some faster field (same bucket), check the - // one-step-faster field for collision. If in collision, limit to current field. - double result_sw_speed = baselinkToSteeringSpeed(result_vel.x, result_vel.tw); - const SubPolygonParameter * result_field = findField(result_sw_speed, target_steering_angle); - if (result_field != nullptr && result_field != current_field && - std::abs(result_sw_speed) > std::abs(current_sw_speed)) - { - bool forward = current_sw_speed >= 0; - auto fields_at_angle = findFieldsForAngle(target_steering_angle, forward); - for (size_t i = 0; i < fields_at_angle.size(); i++) { - if (fields_at_angle[i] == current_field && i + 1 < fields_at_angle.size()) { - const SubPolygonParameter * next_field = fields_at_angle[i + 1]; - debug_msg.next_field_name = next_field->velocity_polygon_name_; - int pts = getPointsInsideSubPolygon(*next_field, collision_points_map); - debug_msg.next_field_collision_pts = pts; - if (pts >= min_points_) { - // Limit steering wheel speed to current field boundary. - // Decompose directly into x and tw to avoid tan(π/2) singularity. - double limit_sw = (current_sw_speed >= 0) ? - current_field->linear_max_ : current_field->linear_min_; - if (std::abs(result_sw_speed) > std::abs(limit_sw)) { - double limited_x = limit_sw * std::cos(target_steering_angle); - double limited_tw = limit_sw * std::sin(target_steering_angle) / wheelbase_; - debug_msg.speed_limit_applied = limited_x; - result_vel.x = limited_x; - result_vel.tw = limited_tw; - modified = true; - } - } - break; + // Check one field up from the current field at the current physical angle. + // - Next field exists and is collision-free → allow + // - Next field exists and has obstacles → limit to current field's max + // - No next field exists → limit to current field's max (prevent e-stop + // beyond defined fields) + bool forward = current_sw_speed >= 0; + auto fields_at_angle = findFieldsForAngle(current_sa, forward); + bool should_limit = false; + + // Find the current field in the sorted list + for (size_t i = 0; i < fields_at_angle.size(); i++) { + if (fields_at_angle[i] != current_field) { + continue; + } + + if (i + 1 >= fields_at_angle.size()) { + // No faster field exists — cap at current field's max + should_limit = true; + } else { + const SubPolygonParameter * next_field = fields_at_angle[i + 1]; + debug_msg.next_field_name = next_field->velocity_polygon_name_; + int pts = getPointsInsideSubPolygon(*next_field, collision_points_map); + debug_msg.next_field_collision_pts = pts; + if (pts >= min_points_) { + // Next field has obstacles — cap at current field's max + should_limit = true; } } + break; } + + if (should_limit) { + double limit_sw = forward ? + current_field->linear_max_ : current_field->linear_min_; + double result_sw_speed = baselinkToSteeringSpeed(result_vel.x, result_vel.tw); + if (std::abs(result_sw_speed) > std::abs(limit_sw)) { + double limited_x = limit_sw * std::cos(current_sa); + double limited_tw = limit_sw * std::sin(current_sa) / wheelbase_; + debug_msg.speed_limit_applied = limited_x; + result_vel.x = limited_x; + result_vel.tw = limited_tw; + modified = true; + } + } + // Same bucket → done if (modified) { robot_action.req_vel = result_vel; From 696927bf91e3f6be6f11dc45b713243423ff7cdf Mon Sep 17 00:00:00 2001 From: juuulianx Date: Thu, 19 Mar 2026 15:48:36 +0100 Subject: [PATCH 10/35] Fixed field limit logic in collision_monitor. --- .../src/velocity_polygon.cpp | 45 +++++++++---------- 1 file changed, 22 insertions(+), 23 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 63e9fc95702..7a9d47003af 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -634,13 +634,15 @@ bool VelocityPolygon::validateSteering( if (same_bucket) { // Check one field up from the current field at the current physical angle. - // - Next field exists and is collision-free → allow + // Always limit speed to at most the next reachable field's max: + // - Next field exists and is collision-free → limit to next field's max // - Next field exists and has obstacles → limit to current field's max - // - No next field exists → limit to current field's max (prevent e-stop - // beyond defined fields) + // - No next field exists → limit to current field's max + // This ensures progressive speed increase (one field per cycle) and + // prevents exceeding defined field ranges into e-stop territory. bool forward = current_sw_speed >= 0; auto fields_at_angle = findFieldsForAngle(current_sa, forward); - bool should_limit = false; + const SubPolygonParameter * limit_field = current_field; // Find the current field in the sorted list for (size_t i = 0; i < fields_at_angle.size(); i++) { @@ -648,34 +650,31 @@ bool VelocityPolygon::validateSteering( continue; } - if (i + 1 >= fields_at_angle.size()) { - // No faster field exists — cap at current field's max - should_limit = true; - } else { + if (i + 1 < fields_at_angle.size()) { const SubPolygonParameter * next_field = fields_at_angle[i + 1]; debug_msg.next_field_name = next_field->velocity_polygon_name_; int pts = getPointsInsideSubPolygon(*next_field, collision_points_map); debug_msg.next_field_collision_pts = pts; - if (pts >= min_points_) { - // Next field has obstacles — cap at current field's max - should_limit = true; + if (pts < min_points_) { + // Next field is collision-free — allow up to next field's max + limit_field = next_field; } + // else: next field has obstacles — stay at current field's max } + // else: no faster field — stay at current field's max break; } - if (should_limit) { - double limit_sw = forward ? - current_field->linear_max_ : current_field->linear_min_; - double result_sw_speed = baselinkToSteeringSpeed(result_vel.x, result_vel.tw); - if (std::abs(result_sw_speed) > std::abs(limit_sw)) { - double limited_x = limit_sw * std::cos(current_sa); - double limited_tw = limit_sw * std::sin(current_sa) / wheelbase_; - debug_msg.speed_limit_applied = limited_x; - result_vel.x = limited_x; - result_vel.tw = limited_tw; - modified = true; - } + double limit_sw = forward ? + limit_field->linear_max_ : limit_field->linear_min_; + double result_sw_speed = baselinkToSteeringSpeed(result_vel.x, result_vel.tw); + if (std::abs(result_sw_speed) > std::abs(limit_sw)) { + double limited_x = limit_sw * std::cos(current_sa); + double limited_tw = limit_sw * std::sin(current_sa) / wheelbase_; + debug_msg.speed_limit_applied = limited_x; + result_vel.x = limited_x; + result_vel.tw = limited_tw; + modified = true; } // Same bucket → done From 45611d3090eef5a155202dda77c38f845e6442d6 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Thu, 19 Mar 2026 16:05:03 +0100 Subject: [PATCH 11/35] Fixed field limit logic in collision_monitor, again. --- nav2_collision_monitor/src/velocity_polygon.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 7a9d47003af..e27883954b7 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -633,6 +633,18 @@ bool VelocityPolygon::validateSteering( debug_msg.same_bucket = same_bucket; if (same_bucket) { + // Skip same-bucket limiting when the robot is at or near standstill. + // The current field at standstill (e.g. creeping/stopped) is not meaningful + // for speed limiting — the robot must be free to start moving. + if (std::abs(current_sw_speed) < low_speed_threshold_) { + debug_msg.modified = false; + debug_msg.result_vel_x = result_vel.x; + debug_msg.result_vel_y = result_vel.y; + debug_msg.result_vel_tw = result_vel.tw; + steering_debug_pub_->publish(debug_msg); + return false; + } + // Check one field up from the current field at the current physical angle. // Always limit speed to at most the next reachable field's max: // - Next field exists and is collision-free → limit to next field's max From 73d814b9cef70f1d057281c79cc4cfcd870c139a Mon Sep 17 00:00:00 2001 From: juuulianx Date: Thu, 19 Mar 2026 16:26:17 +0100 Subject: [PATCH 12/35] Fixed field limit logic in collision_monitor, again, again. --- nav2_collision_monitor/src/velocity_polygon.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index e27883954b7..73484f2e642 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -681,8 +681,8 @@ bool VelocityPolygon::validateSteering( limit_field->linear_max_ : limit_field->linear_min_; double result_sw_speed = baselinkToSteeringSpeed(result_vel.x, result_vel.tw); if (std::abs(result_sw_speed) > std::abs(limit_sw)) { - double limited_x = limit_sw * std::cos(current_sa); - double limited_tw = limit_sw * std::sin(current_sa) / wheelbase_; + double limited_x = limit_sw * std::cos(target_steering_angle); + double limited_tw = limit_sw * std::sin(target_steering_angle) / wheelbase_; debug_msg.speed_limit_applied = limited_x; result_vel.x = limited_x; result_vel.tw = limited_tw; From 746caedc5f7787caf001dbfe0e9d7a1e1bf2e54b Mon Sep 17 00:00:00 2001 From: juuulianx Date: Fri, 20 Mar 2026 14:27:35 +0100 Subject: [PATCH 13/35] Fixed wheelbase value and therefore linear and angular speed calculations. --- nav2_collision_monitor/src/velocity_polygon.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 73484f2e642..bb50f7e8b96 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -62,7 +62,19 @@ bool VelocityPolygon::getParameters( holonomic_ = node->declare_or_get_parameter( polygon_name_ + ".holonomic", false); - wheelbase_ = node->declare_or_get_parameter(polygon_name_ + ".wheelbase", 1.0); + // Try polygon-specific wheelbase first, then fall back to the global + // robot wheelbase parameter (from amr_parameters.yaml), then default 1.0 + double default_wheelbase = 1.0; + try { + default_wheelbase = node->declare_or_get_parameter("wheelbase"); + } catch (const rclcpp::exceptions::ParameterNotDeclaredException &) { + } catch (const rclcpp::exceptions::ParameterUninitializedException &) { + } catch (const rclcpp::exceptions::InvalidParameterValueException &) { + } + wheelbase_ = node->declare_or_get_parameter( + polygon_name_ + ".wheelbase", default_wheelbase); + RCLCPP_INFO( + logger_, "[%s]: Using wheelbase: %.4f m", polygon_name_.c_str(), wheelbase_); low_speed_threshold_ = node->declare_or_get_parameter( polygon_name_ + ".low_speed_threshold", 0.1); From 411a8585d20299a848e57ee7d9d135d2eb07d050 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Fri, 20 Mar 2026 15:49:14 +0100 Subject: [PATCH 14/35] Fixed wheelbase value and therefore linear and angular speed calculations, properly this time. --- .../src/velocity_polygon.cpp | 20 +++++++++---------- nav2_msgs/msg/SteeringValidationDebug.msg | 3 ++- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index bb50f7e8b96..8e864b1bfe8 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -62,19 +62,16 @@ bool VelocityPolygon::getParameters( holonomic_ = node->declare_or_get_parameter( polygon_name_ + ".holonomic", false); - // Try polygon-specific wheelbase first, then fall back to the global - // robot wheelbase parameter (from amr_parameters.yaml), then default 1.0 - double default_wheelbase = 1.0; - try { - default_wheelbase = node->declare_or_get_parameter("wheelbase"); - } catch (const rclcpp::exceptions::ParameterNotDeclaredException &) { - } catch (const rclcpp::exceptions::ParameterUninitializedException &) { - } catch (const rclcpp::exceptions::InvalidParameterValueException &) { - } - wheelbase_ = node->declare_or_get_parameter( - polygon_name_ + ".wheelbase", default_wheelbase); + wheelbase_ = node->declare_or_get_parameter(polygon_name_ + ".wheelbase", 1.0); RCLCPP_INFO( logger_, "[%s]: Using wheelbase: %.4f m", polygon_name_.c_str(), wheelbase_); + if (std::abs(wheelbase_ - 1.0) < 1e-6) { + RCLCPP_WARN( + logger_, + "[%s]: Wheelbase is default (1.0 m). Set '%s.wheelbase' to the robot's " + "actual wheelbase to ensure correct steering wheel speed calculations.", + polygon_name_.c_str(), polygon_name_.c_str()); + } low_speed_threshold_ = node->declare_or_get_parameter( polygon_name_ + ".low_speed_threshold", 0.1); @@ -481,6 +478,7 @@ bool VelocityPolygon::validateSteering( nav2_msgs::msg::SteeringValidationDebug debug_msg; debug_msg.header.stamp = clock_->now(); debug_msg.polygon_name = polygon_name_; + debug_msg.wheelbase = wheelbase_; debug_msg.steering_angle_limit = std::numeric_limits::quiet_NaN(); debug_msg.speed_limit_applied = 0.0; debug_msg.next_field_collision_pts = -1; diff --git a/nav2_msgs/msg/SteeringValidationDebug.msg b/nav2_msgs/msg/SteeringValidationDebug.msg index 6cd759b6f8b..a7a1496e9cd 100644 --- a/nav2_msgs/msg/SteeringValidationDebug.msg +++ b/nav2_msgs/msg/SteeringValidationDebug.msg @@ -70,5 +70,6 @@ float64 result_vel_tw # final output velocity tw float64 speed_limit_applied # baselink speed limit applied (0 if none) float64 steering_angle_limit # steering angle limit applied (NaN if none) -# Polygon name +# Polygon configuration string polygon_name +float64 wheelbase # wheelbase used for steering calculations From 915341a0c88bfb01cfade0570eff6f2e5fd28bf6 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Fri, 20 Mar 2026 15:56:24 +0100 Subject: [PATCH 15/35] Fixed wheelbase value and therefore linear and angular speed calculations, properly this time, without rebuild in nav2_msgs. --- nav2_collision_monitor/src/velocity_polygon.cpp | 1 - nav2_msgs/msg/SteeringValidationDebug.msg | 3 +-- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 8e864b1bfe8..b24e41b9bd2 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -478,7 +478,6 @@ bool VelocityPolygon::validateSteering( nav2_msgs::msg::SteeringValidationDebug debug_msg; debug_msg.header.stamp = clock_->now(); debug_msg.polygon_name = polygon_name_; - debug_msg.wheelbase = wheelbase_; debug_msg.steering_angle_limit = std::numeric_limits::quiet_NaN(); debug_msg.speed_limit_applied = 0.0; debug_msg.next_field_collision_pts = -1; diff --git a/nav2_msgs/msg/SteeringValidationDebug.msg b/nav2_msgs/msg/SteeringValidationDebug.msg index a7a1496e9cd..6cd759b6f8b 100644 --- a/nav2_msgs/msg/SteeringValidationDebug.msg +++ b/nav2_msgs/msg/SteeringValidationDebug.msg @@ -70,6 +70,5 @@ float64 result_vel_tw # final output velocity tw float64 speed_limit_applied # baselink speed limit applied (0 if none) float64 steering_angle_limit # steering angle limit applied (NaN if none) -# Polygon configuration +# Polygon name string polygon_name -float64 wheelbase # wheelbase used for steering calculations From d2cf4dd9137bf281f9ddbc4458ea6c9ee778637b Mon Sep 17 00:00:00 2001 From: juuulianx Date: Wed, 25 Mar 2026 15:55:18 +0100 Subject: [PATCH 16/35] Fixed field discovery in collision_monitor to correctly work with negativ speeds. --- nav2_collision_monitor/src/velocity_polygon.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index b24e41b9bd2..2334c0d805b 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -762,9 +762,13 @@ bool VelocityPolygon::validateSteering( // Find the starting field (fastest that covers max_sw_speed) for (int i = start_idx; i >= 0; i--) { - if (max_sw_speed >= std::abs(neighbour_fields[i]->linear_min_) && - max_sw_speed <= std::abs(neighbour_fields[i]->linear_max_)) - { + double abs_lo = std::min( + std::abs(neighbour_fields[i]->linear_min_), + std::abs(neighbour_fields[i]->linear_max_)); + double abs_hi = std::max( + std::abs(neighbour_fields[i]->linear_min_), + std::abs(neighbour_fields[i]->linear_max_)); + if (max_sw_speed >= abs_lo && max_sw_speed <= abs_hi) { start_idx = i; break; } From 5c61199952bc68a47a93f8ada1d14de0a4c66fef Mon Sep 17 00:00:00 2001 From: juuulianx Date: Thu, 26 Mar 2026 11:05:21 +0100 Subject: [PATCH 17/35] Attempted different fix for backwards driving. --- .../src/velocity_polygon.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 2334c0d805b..f3a5dbe5cf4 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -754,25 +754,25 @@ bool VelocityPolygon::validateSteering( } // Find the fastest field that covers max(|current|, |target|) speed + // Use signed speed so the range check works for both forward and backward fields + // (backward fields have linear_min_ < linear_max_ < 0, so |min| > |max| + // which inverts the abs-based comparison) double max_sw_speed = std::max(std::abs(current_sw_speed), std::abs(target_sw_speed)); + double signed_max_sw = forward ? max_sw_speed : -max_sw_speed; // Find the valid field: start from fastest covering field, walk down const SubPolygonParameter * valid_field = nullptr; int start_idx = static_cast(neighbour_fields.size()) - 1; - // Find the starting field (fastest that covers max_sw_speed) + // Find the starting field (fastest that covers the speed) for (int i = start_idx; i >= 0; i--) { - double abs_lo = std::min( - std::abs(neighbour_fields[i]->linear_min_), - std::abs(neighbour_fields[i]->linear_max_)); - double abs_hi = std::max( - std::abs(neighbour_fields[i]->linear_min_), - std::abs(neighbour_fields[i]->linear_max_)); - if (max_sw_speed >= abs_lo && max_sw_speed <= abs_hi) { + if (signed_max_sw >= neighbour_fields[i]->linear_min_ && + signed_max_sw <= neighbour_fields[i]->linear_max_) + { start_idx = i; break; } - // If max_sw_speed is beyond all fields, start from the fastest + // If speed is beyond all fields, start from the fastest if (i == 0) { start_idx = static_cast(neighbour_fields.size()) - 1; } From 346809f18ed907008b5be8e6ff5f07cc8b4e1e55 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Mon, 30 Mar 2026 13:22:21 +0200 Subject: [PATCH 18/35] linear_limit_ sign fix --- nav2_collision_monitor/src/velocity_polygon.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index f3a5dbe5cf4..37758b2649f 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -243,11 +243,15 @@ void VelocityPolygon::updatePolygon(const Velocity & cmd_vel_in) slowdown_ratio_ = sub_polygon.slowdown_ratio_; if (sub_polygon.use_steering_angle_) { - // Convert linear_limit from steering wheel speed to baselink speed - linear_limit_ = steeringToBaselinkSpeed( - sub_polygon.linear_limit_, current_steering_angle_); + // Convert linear_limit from steering wheel speed to baselink speed. + // Use std::abs because processStopSlowdownLimit divides linear_limit_ + // by the (always-positive) velocity magnitude to compute a ratio. + // Without abs, backward fields produce a negative limit → ratio clamped + // to 0 → unintended full stop instead of proportional slowdown. + linear_limit_ = std::abs(steeringToBaselinkSpeed( + sub_polygon.linear_limit_, current_steering_angle_)); } else { - linear_limit_ = sub_polygon.linear_limit_; + linear_limit_ = std::abs(sub_polygon.linear_limit_); } angular_limit_ = sub_polygon.angular_limit_; time_before_collision_ = sub_polygon.time_before_collision_; From a9fb658ceda5a1a3479f84e096c563fdc5c2ab80 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Mon, 30 Mar 2026 13:46:48 +0200 Subject: [PATCH 19/35] low_speed_threshold fix implemented. --- .../src/velocity_polygon.cpp | 90 +++++++++++++------ 1 file changed, 62 insertions(+), 28 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 37758b2649f..447f10e536c 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -645,19 +645,69 @@ bool VelocityPolygon::validateSteering( target_steering_angle <= current_field->steering_angle_max_; debug_msg.same_bucket = same_bucket; - if (same_bucket) { - // Skip same-bucket limiting when the robot is at or near standstill. - // The current field at standstill (e.g. creeping/stopped) is not meaningful - // for speed limiting — the robot must be free to start moving. - if (std::abs(current_sw_speed) < low_speed_threshold_) { - debug_msg.modified = false; - debug_msg.result_vel_x = result_vel.x; - debug_msg.result_vel_y = result_vel.y; - debug_msg.result_vel_tw = result_vel.tw; - steering_debug_pub_->publish(debug_msg); - return false; + // When the robot is at near-standstill or its speed falls in a gap between + // fields (current_field == nullptr), use the TARGET direction's fields to + // check for obstacles. This prevents blind acceleration from low speed into + // obstructed fields that would otherwise trigger an e-stop. + bool low_speed = current_field == nullptr || + std::abs(current_sw_speed) < low_speed_threshold_; + + if (low_speed) { + bool target_forward = target_sw_speed >= 0; + auto target_fields = findFieldsForAngle(target_steering_angle, target_forward); + + if (!target_fields.empty()) { + // Use the slowest field (index 0) as effective "current" and check one + // step up, mirroring the normal progressive-acceleration logic. + const SubPolygonParameter * limit_field = target_fields[0]; + + int slowest_pts = getPointsInsideSubPolygon(*target_fields[0], collision_points_map); + debug_msg.next_field_collision_pts = slowest_pts; + + if (slowest_pts >= min_points_) { + // Even the slowest field has obstacles — force stop + result_vel = {0.0, 0.0, 0.0}; + modified = true; + } else { + // Slowest field clear — check one step up + if (target_fields.size() > 1) { + debug_msg.next_field_name = target_fields[1]->velocity_polygon_name_; + int next_pts = getPointsInsideSubPolygon(*target_fields[1], collision_points_map); + debug_msg.next_field_collision_pts = next_pts; + if (next_pts < min_points_) { + limit_field = target_fields[1]; + } + } + + double limit_sw = target_forward ? + limit_field->linear_max_ : limit_field->linear_min_; + double result_sw_speed = baselinkToSteeringSpeed(result_vel.x, result_vel.tw); + if (std::abs(result_sw_speed) > std::abs(limit_sw)) { + double limited_x = limit_sw * std::cos(target_steering_angle); + double limited_tw = limit_sw * std::sin(target_steering_angle) / wheelbase_; + debug_msg.speed_limit_applied = limited_x; + result_vel.x = limited_x; + result_vel.tw = limited_tw; + modified = true; + } + } + + if (modified) { + robot_action.req_vel = result_vel; + robot_action.polygon_name = polygon_name_; + robot_action.action_type = LIMIT; + } } + debug_msg.modified = modified; + debug_msg.result_vel_x = result_vel.x; + debug_msg.result_vel_y = result_vel.y; + debug_msg.result_vel_tw = result_vel.tw; + steering_debug_pub_->publish(debug_msg); + return modified; + } + + if (same_bucket) { // Check one field up from the current field at the current physical angle. // Always limit speed to at most the next reachable field's max: // - Next field exists and is collision-free → limit to next field's max @@ -717,23 +767,7 @@ bool VelocityPolygon::validateSteering( } // 3. Different bucket — find neighbouring bucket (one step in steering direction) - if (current_field == nullptr) { - RCLCPP_WARN( - logger_, - "[%s] validateSteering: current_field is null — no field matches current velocity. " - "odom_vel=(%.3f, %.3f, %.3f), current_sw_speed=%.3f, current_sa=%.3f, " - "cmd_vel=(%.3f, %.3f, %.3f), target_sw_speed=%.3f, target_sa=%.3f. " - "Check that velocity polygon field ranges cover all reachable velocities.", - polygon_name_.c_str(), - odom_vel.x, odom_vel.y, odom_vel.tw, current_sw_speed, current_sa, - cmd_vel_in.x, cmd_vel_in.y, cmd_vel_in.tw, target_sw_speed, target_steering_angle); - debug_msg.modified = false; - debug_msg.result_vel_x = result_vel.x; - debug_msg.result_vel_y = result_vel.y; - debug_msg.result_vel_tw = result_vel.tw; - steering_debug_pub_->publish(debug_msg); - return false; - } + // current_field is guaranteed non-null here (low_speed case handled above) double neighbour_angle; if (target_steering_angle > current_sa) { From 9fd1a51462028f8a2522bb35015291ac19c134c4 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Mon, 30 Mar 2026 15:36:55 +0200 Subject: [PATCH 20/35] Added small delay to collision_monitor field limiting to prevent oscillations. --- .../velocity_polygon.hpp | 19 +++++ .../src/velocity_polygon.cpp | 69 ++++++++++++++++--- 2 files changed, 79 insertions(+), 9 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp index a405740f53f..e02bffccabd 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp @@ -215,6 +215,17 @@ class VelocityPolygon : public Polygon const SubPolygonParameter & sub_polygon, const std::unordered_map> & collision_points_map) const; + /// @brief Check if a sub-polygon is a creeping field (always allowed) + static bool isCreepingField(const SubPolygonParameter & sp); + + /// @brief Check if the next field has been stably free of obstacles + /// for at least field_free_duration_ seconds. Returns the raw point + /// count in \p pts_out so callers can still use it for debug messages. + bool isNextFieldStablyFree( + const SubPolygonParameter & field, + const std::unordered_map> & collision_points_map, + int & pts_out); + // Clock rclcpp::Clock::SharedPtr clock_; // Debug publisher for steering validation @@ -230,6 +241,14 @@ class VelocityPolygon : public Polygon double wheelbase_; /// @brief Speed below which steering is freely allowed double low_speed_threshold_; + /// @brief Duration (seconds) the next field must be continuously free before + /// allowing acceleration into it. Prevents oscillation from transient + /// sensor readings near field boundaries. + double field_free_duration_; + /// @brief Timestamp when the currently-monitored next field first became free + rclcpp::Time next_field_free_stamp_; + /// @brief Name of the field being monitored for the free-duration check + std::string monitored_next_field_; /// @brief Vector to store the parameters of the sub-polygon std::vector sub_polygons_; }; // class VelocityPolygon diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 447f10e536c..60908ac7aea 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -76,6 +76,13 @@ bool VelocityPolygon::getParameters( low_speed_threshold_ = node->declare_or_get_parameter( polygon_name_ + ".low_speed_threshold", 0.1); + field_free_duration_ = node->declare_or_get_parameter( + polygon_name_ + ".field_free_duration", 0.1); + RCLCPP_INFO( + logger_, "[%s]: Field free duration: %.3f s", polygon_name_.c_str(), field_free_duration_); + + next_field_free_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + for (std::string velocity_polygon_name : velocity_polygons) { // polygon points parameter std::vector poly; @@ -468,6 +475,38 @@ int VelocityPolygon::getPointsInsideSubPolygon( return num; } +bool VelocityPolygon::isCreepingField(const SubPolygonParameter & sp) +{ + return sp.velocity_polygon_name_.find("creeping") != std::string::npos; +} + +bool VelocityPolygon::isNextFieldStablyFree( + const SubPolygonParameter & field, + const std::unordered_map> & collision_points_map, + int & pts_out) +{ + pts_out = getPointsInsideSubPolygon(field, collision_points_map); + bool currently_free = pts_out < min_points_; + + // Reset timer when the monitored field changes + if (monitored_next_field_ != field.velocity_polygon_name_) { + monitored_next_field_ = field.velocity_polygon_name_; + next_field_free_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + } + + if (!currently_free) { + next_field_free_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + return false; + } + + // Field is currently free — check if it has been free long enough + if (next_field_free_stamp_.nanoseconds() == 0) { + next_field_free_stamp_ = clock_->now(); + } + return (clock_->now() - next_field_free_stamp_) >= + rclcpp::Duration::from_seconds(field_free_duration_); +} + bool VelocityPolygon::validateSteering( const Velocity & cmd_vel_in, const Velocity & odom_vel, @@ -664,17 +703,20 @@ bool VelocityPolygon::validateSteering( int slowest_pts = getPointsInsideSubPolygon(*target_fields[0], collision_points_map); debug_msg.next_field_collision_pts = slowest_pts; - if (slowest_pts >= min_points_) { - // Even the slowest field has obstacles — force stop + if (slowest_pts >= min_points_ && !isCreepingField(*target_fields[0])) { + // Slowest non-creeping field has obstacles — force stop result_vel = {0.0, 0.0, 0.0}; modified = true; } else { - // Slowest field clear — check one step up + // Slowest field is clear (or is a creeping field — always allowed). + // Check one step up using the stable-free timer to prevent oscillation. if (target_fields.size() > 1) { debug_msg.next_field_name = target_fields[1]->velocity_polygon_name_; - int next_pts = getPointsInsideSubPolygon(*target_fields[1], collision_points_map); + int next_pts; + bool stably_free = isNextFieldStablyFree( + *target_fields[1], collision_points_map, next_pts); debug_msg.next_field_collision_pts = next_pts; - if (next_pts < min_points_) { + if (stably_free) { limit_field = target_fields[1]; } } @@ -728,13 +770,22 @@ bool VelocityPolygon::validateSteering( if (i + 1 < fields_at_angle.size()) { const SubPolygonParameter * next_field = fields_at_angle[i + 1]; debug_msg.next_field_name = next_field->velocity_polygon_name_; - int pts = getPointsInsideSubPolygon(*next_field, collision_points_map); + int pts; + bool stably_free = isNextFieldStablyFree( + *next_field, collision_points_map, pts); debug_msg.next_field_collision_pts = pts; - if (pts < min_points_) { - // Next field is collision-free — allow up to next field's max + if (stably_free) { + // Next field has been stably collision-free — allow up to next field's max limit_field = next_field; } - // else: next field has obstacles — stay at current field's max + // else: next field has obstacles or not free long enough — stay at current field's max + } + // Always allow creeping fields: if the current field is a creeping field, + // do not restrict to its max even when the next field is occupied. + if (isCreepingField(*current_field) && i + 1 < fields_at_angle.size()) { + // Override limit to at least the next field's boundary so the robot + // can always leave a creeping field. + limit_field = fields_at_angle[i + 1]; } // else: no faster field — stay at current field's max break; From c54128097bbec8930ce4032f7830d61f992e77f9 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Mon, 30 Mar 2026 15:56:11 +0200 Subject: [PATCH 21/35] Fixed another issue in CM. --- .../src/velocity_polygon.cpp | 33 ++++++++++++++----- 1 file changed, 25 insertions(+), 8 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 60908ac7aea..2cf2b9668ad 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -710,6 +710,7 @@ bool VelocityPolygon::validateSteering( } else { // Slowest field is clear (or is a creeping field — always allowed). // Check one step up using the stable-free timer to prevent oscillation. + bool next_occupied = false; if (target_fields.size() > 1) { debug_msg.next_field_name = target_fields[1]->velocity_polygon_name_; int next_pts; @@ -718,11 +719,21 @@ bool VelocityPolygon::validateSteering( debug_msg.next_field_collision_pts = next_pts; if (stably_free) { limit_field = target_fields[1]; + } else { + next_occupied = true; } } double limit_sw = target_forward ? limit_field->linear_max_ : limit_field->linear_min_; + + // When the next field is occupied, use linear_limit (safety speed) + if (next_occupied) { + double limit_cap = std::abs(limit_field->linear_limit_); + if (limit_cap > 0.0 && limit_cap < std::abs(limit_sw)) { + limit_sw = target_forward ? limit_cap : -limit_cap; + } + } double result_sw_speed = baselinkToSteeringSpeed(result_vel.x, result_vel.tw); if (std::abs(result_sw_speed) > std::abs(limit_sw)) { double limited_x = limit_sw * std::cos(target_steering_angle); @@ -760,6 +771,7 @@ bool VelocityPolygon::validateSteering( bool forward = current_sw_speed >= 0; auto fields_at_angle = findFieldsForAngle(current_sa, forward); const SubPolygonParameter * limit_field = current_field; + bool next_field_occupied = false; // Find the current field in the sorted list for (size_t i = 0; i < fields_at_angle.size(); i++) { @@ -777,15 +789,9 @@ bool VelocityPolygon::validateSteering( if (stably_free) { // Next field has been stably collision-free — allow up to next field's max limit_field = next_field; + } else { + next_field_occupied = true; } - // else: next field has obstacles or not free long enough — stay at current field's max - } - // Always allow creeping fields: if the current field is a creeping field, - // do not restrict to its max even when the next field is occupied. - if (isCreepingField(*current_field) && i + 1 < fields_at_angle.size()) { - // Override limit to at least the next field's boundary so the robot - // can always leave a creeping field. - limit_field = fields_at_angle[i + 1]; } // else: no faster field — stay at current field's max break; @@ -793,6 +799,17 @@ bool VelocityPolygon::validateSteering( double limit_sw = forward ? limit_field->linear_max_ : limit_field->linear_min_; + + // When the next field is occupied, use the current field's linear_limit + // (safety speed) instead of its full range boundary. Obstacles are clearly + // nearby if the next field has collision points, so the more conservative + // limit is appropriate even when the current polygon doesn't detect them. + if (next_field_occupied) { + double limit_cap = std::abs(current_field->linear_limit_); + if (limit_cap > 0.0 && limit_cap < std::abs(limit_sw)) { + limit_sw = forward ? limit_cap : -limit_cap; + } + } double result_sw_speed = baselinkToSteeringSpeed(result_vel.x, result_vel.tw); if (std::abs(result_sw_speed) > std::abs(limit_sw)) { double limited_x = limit_sw * std::cos(target_steering_angle); From 1e4cc83df4a59fcfee97a288b6029222498bae18 Mon Sep 17 00:00:00 2001 From: Johannes Plapp Date: Mon, 30 Mar 2026 16:10:16 +0200 Subject: [PATCH 22/35] Review fixes: deduplicate isPointInsidePoly, fix log levels - Replace duplicated ray-casting in VelocityPolygon::isPointInsidePoly with nav2_util::geometry_utils::isPointInsidePolygon - Revert range out-of-scope log back to RCLCPP_DEBUG (noisy sensor data) - Remove "Source is disabled" WARN log (normal configuration state) Co-Authored-By: Claude Opus 4.6 (1M context) --- .../src/collision_monitor_node.cpp | 5 ----- nav2_collision_monitor/src/range.cpp | 4 ++-- .../src/velocity_polygon.cpp | 20 ++----------------- 3 files changed, 4 insertions(+), 25 deletions(-) diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 248df8c62ab..8185f7cd738 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -471,11 +471,6 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: "[%s]: Source enabled and getData() succeeded but returned 0 points", source->getSourceName().c_str()); } - } else { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 2000, - "[%s]: Source is disabled — skipping getData(), 0 points contributed", - source->getSourceName().c_str()); } if (collision_points_marker_pub_->get_subscription_count() > 0) { diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index e078d339aaf..6ca08566f12 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -85,8 +85,8 @@ bool Range::getData( // Ignore data, if its range is out of scope of range sensor abilities if (data_->range < data_->min_range || data_->range > data_->max_range) { - RCLCPP_WARN_THROTTLE( - logger_, *clock_, 2000, + RCLCPP_DEBUG( + logger_, "[%s]: Data range %fm is out of {%f..%f} sensor span. Ignoring...", source_name_.c_str(), data_->range, data_->min_range, data_->max_range); return false; diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 2cf2b9668ad..0acb26ab11a 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -19,6 +19,7 @@ #include #include "nav2_ros_common/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" namespace nav2_collision_monitor { @@ -434,24 +435,7 @@ VelocityPolygon::findFieldsForAngle(double steering_angle, bool forward) const bool VelocityPolygon::isPointInsidePoly( const Point & point, const std::vector & vertices) { - // Ray-casting algorithm (same as Polygon::isPointInside but for arbitrary vertices) - const int poly_size = static_cast(vertices.size()); - int i, j; - bool res = false; - - i = poly_size - 1; - for (j = 0; j < poly_size; j++) { - if ((point.y <= vertices[i].y) == (point.y > vertices[j].y)) { - const double x_inter = vertices[i].x + - (point.y - vertices[i].y) * (vertices[j].x - vertices[i].x) / - (vertices[j].y - vertices[i].y); - if (x_inter > point.x) { - res = !res; - } - } - i = j; - } - return res; + return nav2_util::geometry_utils::isPointInsidePolygon(point.x, point.y, vertices); } int VelocityPolygon::getPointsInsideSubPolygon( From e737f25103c2a80e0919b52f13645a87b6ebf48e Mon Sep 17 00:00:00 2001 From: juuulianx Date: Mon, 30 Mar 2026 16:41:39 +0200 Subject: [PATCH 23/35] Attemped another fix in collision_monitor logic. --- .../src/collision_monitor_node.cpp | 8 +++++-- .../src/velocity_polygon.cpp | 21 +++++++++++++------ 2 files changed, 21 insertions(+), 8 deletions(-) diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 248df8c62ab..0877579ae9b 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -548,10 +548,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) + // Track the active LIMIT VelocityPolygon. + // Always set it (even when subpolygon is "none" / velocity not covered) + // so that validateSteering runs and the low_speed path can check + // target fields for obstacles. Without this, uncovered velocities + // have no field-based obstacle protection. if (polygon->getActionType() == LIMIT) { auto vp = std::dynamic_pointer_cast(polygon); - if (vp && vp->getCurrentSubPolygonName() != "none") { + if (vp) { active_limit_vel_polygon = vp; } } diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 2cf2b9668ad..e8f5ffd7e1c 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -269,6 +269,11 @@ void VelocityPolygon::updatePolygon(const Velocity & cmd_vel_in) current_subpolygon_name_ = "none"; + // Clear the polygon so processStopSlowdownLimit doesn't use a stale shape + // from a previously matched (and now wrong) sub-polygon. + // validateSteering's low_speed path still provides protection via target-field checks. + poly_.clear(); + // Log for uncovered velocity RCLCPP_WARN_THROTTLE( logger_, *clock_, 2.0, @@ -280,6 +285,10 @@ void VelocityPolygon::updatePolygon(const Velocity & cmd_vel_in) bool VelocityPolygon::isInRange( const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon) { + // Small tolerance for field boundary comparisons to prevent velocities from + // falling into gaps between adjacent fields due to float precision. + constexpr double kBoundaryEps = 0.01; + if (sub_polygon.use_steering_angle_) { current_steering_angle_ = computeSteeringAngle(cmd_vel_in); @@ -299,23 +308,23 @@ bool VelocityPolygon::isInRange( ); // Check linear range using steering wheel speed - bool in_range = steering_wheel_speed <= sub_polygon.linear_max_ && - steering_wheel_speed >= sub_polygon.linear_min_; + bool in_range = steering_wheel_speed <= sub_polygon.linear_max_ + kBoundaryEps && + steering_wheel_speed >= sub_polygon.linear_min_ - kBoundaryEps; if (!in_range) { return false; } // Check steering angle range - in_range &= current_steering_angle_ <= sub_polygon.steering_angle_max_ && - current_steering_angle_ >= sub_polygon.steering_angle_min_; + in_range &= current_steering_angle_ <= sub_polygon.steering_angle_max_ + kBoundaryEps && + current_steering_angle_ >= sub_polygon.steering_angle_min_ - kBoundaryEps; return in_range; } // Non-steering-angle mode: use baselink speed directly - bool in_range = cmd_vel_in.x <= sub_polygon.linear_max_ && - cmd_vel_in.x >= sub_polygon.linear_min_; + bool in_range = cmd_vel_in.x <= sub_polygon.linear_max_ + kBoundaryEps && + cmd_vel_in.x >= sub_polygon.linear_min_ - kBoundaryEps; if (!in_range) { return false; From 397445a8eaedc5de2c0c53760180600e332767b4 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Mon, 30 Mar 2026 17:34:19 +0200 Subject: [PATCH 24/35] fixed creeping issue. --- nav2_collision_monitor/src/velocity_polygon.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 8d3a1437055..78cdbf9ac07 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -697,9 +697,18 @@ bool VelocityPolygon::validateSteering( debug_msg.next_field_collision_pts = slowest_pts; if (slowest_pts >= min_points_ && !isCreepingField(*target_fields[0])) { - // Slowest non-creeping field has obstacles — force stop - result_vel = {0.0, 0.0, 0.0}; - modified = true; + // Slowest non-creeping field has obstacles — limit to creeping speed + // so the robot can always maneuver slowly, even when the way ahead + // is blocked at higher speeds. + double creep_sw = low_speed_threshold_; + double sign = target_forward ? 1.0 : -1.0; + double result_sw = baselinkToSteeringSpeed(result_vel.x, result_vel.tw); + if (std::abs(result_sw) > creep_sw) { + result_vel.x = sign * creep_sw * std::cos(target_steering_angle); + result_vel.tw = sign * creep_sw * std::sin(target_steering_angle) / wheelbase_; + debug_msg.speed_limit_applied = result_vel.x; + modified = true; + } } else { // Slowest field is clear (or is a creeping field — always allowed). // Check one step up using the stable-free timer to prevent oscillation. From 7e088cf4b26b8264f1ca7098263b3d9c1bcdda0b Mon Sep 17 00:00:00 2001 From: Johannes Plapp Date: Tue, 31 Mar 2026 08:17:18 +0200 Subject: [PATCH 25/35] Slim down debug msg, use float32, add processing time topic - Remove source filtering and bounding box diagnostics from SteeringValidationDebug msg and validateSteering code - Switch all float64 fields to float32 in debug msg - Add ~/processing_time_ms (Float32) topic to collision monitor, published at the end of each process() cycle using steady_clock Co-Authored-By: Claude Opus 4.6 (1M context) --- .../collision_monitor_node.hpp | 3 + .../src/collision_monitor_node.cpp | 10 +++ .../src/velocity_polygon.cpp | 72 +------------------ nav2_msgs/msg/SteeringValidationDebug.msg | 61 ++++++---------- 4 files changed, 36 insertions(+), 110 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 1daf3ee3b45..8d0cb70040d 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/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" @@ -271,6 +272,8 @@ class CollisionMonitor : public nav2::LifecycleNode bool enable_steering_validation_; /// @brief Active polygons publisher rclcpp::Publisher::SharedPtr active_polygons_pub_; + /// @brief Processing time publisher (ms) + rclcpp::Publisher::SharedPtr processing_time_pub_; }; // class CollisionMonitor } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 9b9088bc54d..07ad6c094d9 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -14,6 +14,7 @@ #include "nav2_collision_monitor/collision_monitor_node.hpp" +#include #include #include #include @@ -81,6 +82,8 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state) cmd_vel_out_pub_ = std::make_unique(node, cmd_vel_out_topic); active_polygons_pub_ = this->create_publisher( "~/active_velocity_polygons"); + processing_time_pub_ = this->create_publisher( + "~/processing_time_ms", rclcpp::QoS(1)); if (!state_topic.empty()) { state_pub_ = this->create_publisher( @@ -429,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_) { @@ -622,6 +626,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( + std::chrono::steady_clock::now() - process_start).count(); + processing_time_pub_->publish(time_msg); } bool CollisionMonitor::processStopSlowdownLimit( diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 78cdbf9ac07..0a6ce58b93b 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -514,7 +514,7 @@ bool VelocityPolygon::validateSteering( nav2_msgs::msg::SteeringValidationDebug debug_msg; debug_msg.header.stamp = clock_->now(); debug_msg.polygon_name = polygon_name_; - debug_msg.steering_angle_limit = std::numeric_limits::quiet_NaN(); + debug_msg.steering_angle_limit = std::numeric_limits::quiet_NaN(); debug_msg.speed_limit_applied = 0.0; debug_msg.next_field_collision_pts = -1; debug_msg.neighbour_collision_pts = -1; @@ -531,76 +531,6 @@ bool VelocityPolygon::validateSteering( debug_msg.step1_req_vel_y = robot_action.req_vel.y; debug_msg.step1_req_vel_tw = robot_action.req_vel.tw; - // Source filtering diagnostics - debug_msg.step1_configured_sources = getSourcesNames(); - debug_msg.step1_polygon_vertex_count = static_cast(poly_.size()); - int configured_pts = 0; - for (const auto & src_name : debug_msg.step1_configured_sources) { - auto it = collision_points_map.find(src_name); - if (it != collision_points_map.end()) { - configured_pts += static_cast(it->second.size()); - } - } - debug_msg.step1_configured_source_pts = configured_pts; - int all_pts = 0; - for (const auto & kv : collision_points_map) { - all_pts += static_cast(kv.second.size()); - } - debug_msg.step1_all_source_pts = all_pts; - - // Bounding box of polygon vertices - double poly_min_x = std::numeric_limits::max(); - double poly_max_x = std::numeric_limits::lowest(); - double poly_min_y = std::numeric_limits::max(); - double poly_max_y = std::numeric_limits::lowest(); - for (const auto & v : poly_) { - poly_min_x = std::min(poly_min_x, v.x); - poly_max_x = std::max(poly_max_x, v.x); - poly_min_y = std::min(poly_min_y, v.y); - poly_max_y = std::max(poly_max_y, v.y); - } - debug_msg.step1_poly_min_x = poly_min_x; - debug_msg.step1_poly_max_x = poly_max_x; - debug_msg.step1_poly_min_y = poly_min_y; - debug_msg.step1_poly_max_y = poly_max_y; - - // Bounding box of collision points from configured sources - double pts_min_x = std::numeric_limits::max(); - double pts_max_x = std::numeric_limits::lowest(); - double pts_min_y = std::numeric_limits::max(); - double pts_max_y = std::numeric_limits::lowest(); - for (const auto & src_name : debug_msg.step1_configured_sources) { - auto it = collision_points_map.find(src_name); - if (it != collision_points_map.end()) { - for (const auto & pt : it->second) { - pts_min_x = std::min(pts_min_x, pt.x); - pts_max_x = std::max(pts_max_x, pt.x); - pts_min_y = std::min(pts_min_y, pt.y); - pts_max_y = std::max(pts_max_y, pt.y); - } - } - } - debug_msg.step1_pts_min_x = pts_min_x; - debug_msg.step1_pts_max_x = pts_max_x; - debug_msg.step1_pts_min_y = pts_min_y; - debug_msg.step1_pts_max_y = pts_max_y; - - // Count points within the polygon's bounding box - int pts_in_bbox = 0; - for (const auto & src_name : debug_msg.step1_configured_sources) { - auto it = collision_points_map.find(src_name); - if (it != collision_points_map.end()) { - for (const auto & pt : it->second) { - if (pt.x >= poly_min_x && pt.x <= poly_max_x && - pt.y >= poly_min_y && pt.y <= poly_max_y) - { - pts_in_bbox++; - } - } - } - } - debug_msg.step1_pts_in_bbox = pts_in_bbox; - const double target_speed = cmd_vel_in.x; const double current_speed = odom_vel.x; diff --git a/nav2_msgs/msg/SteeringValidationDebug.msg b/nav2_msgs/msg/SteeringValidationDebug.msg index 6cd759b6f8b..b630b5a8b50 100644 --- a/nav2_msgs/msg/SteeringValidationDebug.msg +++ b/nav2_msgs/msg/SteeringValidationDebug.msg @@ -2,51 +2,34 @@ std_msgs/Header header # Input velocities -float64 target_speed # cmd_vel_in.x (baselink) -float64 current_speed # odom_vel.x (baselink) -float64 target_steering_angle # computed from cmd_vel_in -float64 current_steering_angle # computed from odom_vel -float64 target_sw_speed # target steering wheel speed -float64 current_sw_speed # current steering wheel speed +float32 target_speed # cmd_vel_in.x (baselink) +float32 current_speed # odom_vel.x (baselink) +float32 target_steering_angle # computed from cmd_vel_in +float32 current_steering_angle # computed from odom_vel +float32 target_sw_speed # target steering wheel speed +float32 current_sw_speed # current steering wheel speed # cmd_vel_in components -float64 cmd_vel_x -float64 cmd_vel_y -float64 cmd_vel_tw +float32 cmd_vel_x +float32 cmd_vel_y +float32 cmd_vel_tw # odom_vel components -float64 odom_vel_x -float64 odom_vel_y -float64 odom_vel_tw +float32 odom_vel_x +float32 odom_vel_y +float32 odom_vel_tw # Step 1 info (LIMIT polygon collision check before steering validation) string step1_active_sub_polygon # sub-polygon name active during step 1 int32 step1_points_inside # collision points found inside the active polygon int32 step1_min_points # threshold for triggering action bool step1_shape_set # whether polygon shape was set -float64 step1_linear_limit # linear limit of the active sub-polygon (baselink) -float64 step1_angular_limit # angular limit of the active sub-polygon +float32 step1_linear_limit # linear limit of the active sub-polygon (baselink) +float32 step1_angular_limit # angular limit of the active sub-polygon uint8 step1_action_type # action type after step 1 (0=DO_NOTHING,1=STOP,2=SLOWDOWN,3=APPROACH,4=LIMIT) -float64 step1_req_vel_x # velocity x after step 1 -float64 step1_req_vel_y # velocity y after step 1 -float64 step1_req_vel_tw # velocity tw after step 1 - -# Source filtering diagnostics -string[] step1_configured_sources # source names this polygon is configured to check -int32 step1_configured_source_pts # total points from configured sources (before polygon test) -int32 step1_all_source_pts # total points across ALL sources in the map -int32 step1_polygon_vertex_count # number of vertices in the active polygon shape - -# Bounding box diagnostics — compare polygon bounds vs collision point bounds -float64 step1_poly_min_x -float64 step1_poly_max_x -float64 step1_poly_min_y -float64 step1_poly_max_y -float64 step1_pts_min_x # min x of all collision points from configured sources -float64 step1_pts_max_x -float64 step1_pts_min_y -float64 step1_pts_max_y -int32 step1_pts_in_bbox # points within the polygon's bounding box (but not necessarily inside polygon) +float32 step1_req_vel_x # velocity x after step 1 +float32 step1_req_vel_y # velocity y after step 1 +float32 step1_req_vel_tw # velocity tw after step 1 # Step 2 decision path bool crosses_zero # direction reversal detected @@ -62,13 +45,13 @@ int32 neighbour_collision_pts # points inside neighbour start field (diff-buck # Modification result bool modified # whether velocity was modified by step 2 -float64 result_vel_x # final output velocity x -float64 result_vel_y # final output velocity y -float64 result_vel_tw # final output velocity tw +float32 result_vel_x # final output velocity x +float32 result_vel_y # final output velocity y +float32 result_vel_tw # final output velocity tw # Limits applied -float64 speed_limit_applied # baselink speed limit applied (0 if none) -float64 steering_angle_limit # steering angle limit applied (NaN if none) +float32 speed_limit_applied # baselink speed limit applied (0 if none) +float32 steering_angle_limit # steering angle limit applied (NaN if none) # Polygon name string polygon_name From 1135414fd87b024770f7bc0b1be71fa6e4b9a345 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Tue, 31 Mar 2026 11:51:46 +0200 Subject: [PATCH 26/35] Removed my previous changes, but preserved others changes. --- .../velocity_polygon.hpp | 19 -- .../src/collision_monitor_node.cpp | 8 +- .../src/velocity_polygon.cpp | 218 ++++-------------- 3 files changed, 47 insertions(+), 198 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp index e02bffccabd..a405740f53f 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp @@ -215,17 +215,6 @@ class VelocityPolygon : public Polygon const SubPolygonParameter & sub_polygon, const std::unordered_map> & collision_points_map) const; - /// @brief Check if a sub-polygon is a creeping field (always allowed) - static bool isCreepingField(const SubPolygonParameter & sp); - - /// @brief Check if the next field has been stably free of obstacles - /// for at least field_free_duration_ seconds. Returns the raw point - /// count in \p pts_out so callers can still use it for debug messages. - bool isNextFieldStablyFree( - const SubPolygonParameter & field, - const std::unordered_map> & collision_points_map, - int & pts_out); - // Clock rclcpp::Clock::SharedPtr clock_; // Debug publisher for steering validation @@ -241,14 +230,6 @@ class VelocityPolygon : public Polygon double wheelbase_; /// @brief Speed below which steering is freely allowed double low_speed_threshold_; - /// @brief Duration (seconds) the next field must be continuously free before - /// allowing acceleration into it. Prevents oscillation from transient - /// sensor readings near field boundaries. - double field_free_duration_; - /// @brief Timestamp when the currently-monitored next field first became free - rclcpp::Time next_field_free_stamp_; - /// @brief Name of the field being monitored for the free-duration check - std::string monitored_next_field_; /// @brief Vector to store the parameters of the sub-polygon std::vector sub_polygons_; }; // class VelocityPolygon diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 07ad6c094d9..bb386ffd5b7 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -547,14 +547,10 @@ 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. - // Always set it (even when subpolygon is "none" / velocity not covered) - // so that validateSteering runs and the low_speed path can check - // target fields for obstacles. Without this, uncovered velocities - // have no field-based obstacle protection. + // Track the active LIMIT VelocityPolygon (the one matching current speed/steering angle) if (polygon->getActionType() == LIMIT) { auto vp = std::dynamic_pointer_cast(polygon); - if (vp) { + if (vp && vp->getCurrentSubPolygonName() != "none") { active_limit_vel_polygon = vp; } } diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 0a6ce58b93b..4c8912fe3b7 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -77,13 +77,6 @@ bool VelocityPolygon::getParameters( low_speed_threshold_ = node->declare_or_get_parameter( polygon_name_ + ".low_speed_threshold", 0.1); - field_free_duration_ = node->declare_or_get_parameter( - polygon_name_ + ".field_free_duration", 0.1); - RCLCPP_INFO( - logger_, "[%s]: Field free duration: %.3f s", polygon_name_.c_str(), field_free_duration_); - - next_field_free_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); - for (std::string velocity_polygon_name : velocity_polygons) { // polygon points parameter std::vector poly; @@ -251,15 +244,11 @@ void VelocityPolygon::updatePolygon(const Velocity & cmd_vel_in) slowdown_ratio_ = sub_polygon.slowdown_ratio_; if (sub_polygon.use_steering_angle_) { - // Convert linear_limit from steering wheel speed to baselink speed. - // Use std::abs because processStopSlowdownLimit divides linear_limit_ - // by the (always-positive) velocity magnitude to compute a ratio. - // Without abs, backward fields produce a negative limit → ratio clamped - // to 0 → unintended full stop instead of proportional slowdown. - linear_limit_ = std::abs(steeringToBaselinkSpeed( - sub_polygon.linear_limit_, current_steering_angle_)); + // Convert linear_limit from steering wheel speed to baselink speed + linear_limit_ = steeringToBaselinkSpeed( + sub_polygon.linear_limit_, current_steering_angle_); } else { - linear_limit_ = std::abs(sub_polygon.linear_limit_); + linear_limit_ = sub_polygon.linear_limit_; } angular_limit_ = sub_polygon.angular_limit_; time_before_collision_ = sub_polygon.time_before_collision_; @@ -270,11 +259,6 @@ void VelocityPolygon::updatePolygon(const Velocity & cmd_vel_in) current_subpolygon_name_ = "none"; - // Clear the polygon so processStopSlowdownLimit doesn't use a stale shape - // from a previously matched (and now wrong) sub-polygon. - // validateSteering's low_speed path still provides protection via target-field checks. - poly_.clear(); - // Log for uncovered velocity RCLCPP_WARN_THROTTLE( logger_, *clock_, 2.0, @@ -286,10 +270,6 @@ void VelocityPolygon::updatePolygon(const Velocity & cmd_vel_in) bool VelocityPolygon::isInRange( const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon) { - // Small tolerance for field boundary comparisons to prevent velocities from - // falling into gaps between adjacent fields due to float precision. - constexpr double kBoundaryEps = 0.01; - if (sub_polygon.use_steering_angle_) { current_steering_angle_ = computeSteeringAngle(cmd_vel_in); @@ -309,23 +289,23 @@ bool VelocityPolygon::isInRange( ); // Check linear range using steering wheel speed - bool in_range = steering_wheel_speed <= sub_polygon.linear_max_ + kBoundaryEps && - steering_wheel_speed >= sub_polygon.linear_min_ - kBoundaryEps; + bool in_range = steering_wheel_speed <= sub_polygon.linear_max_ && + steering_wheel_speed >= sub_polygon.linear_min_; if (!in_range) { return false; } // Check steering angle range - in_range &= current_steering_angle_ <= sub_polygon.steering_angle_max_ + kBoundaryEps && - current_steering_angle_ >= sub_polygon.steering_angle_min_ - kBoundaryEps; + in_range &= current_steering_angle_ <= sub_polygon.steering_angle_max_ && + current_steering_angle_ >= sub_polygon.steering_angle_min_; return in_range; } // Non-steering-angle mode: use baselink speed directly - bool in_range = cmd_vel_in.x <= sub_polygon.linear_max_ + kBoundaryEps && - cmd_vel_in.x >= sub_polygon.linear_min_ - kBoundaryEps; + bool in_range = cmd_vel_in.x <= sub_polygon.linear_max_ && + cmd_vel_in.x >= sub_polygon.linear_min_; if (!in_range) { return false; @@ -468,38 +448,6 @@ int VelocityPolygon::getPointsInsideSubPolygon( return num; } -bool VelocityPolygon::isCreepingField(const SubPolygonParameter & sp) -{ - return sp.velocity_polygon_name_.find("creeping") != std::string::npos; -} - -bool VelocityPolygon::isNextFieldStablyFree( - const SubPolygonParameter & field, - const std::unordered_map> & collision_points_map, - int & pts_out) -{ - pts_out = getPointsInsideSubPolygon(field, collision_points_map); - bool currently_free = pts_out < min_points_; - - // Reset timer when the monitored field changes - if (monitored_next_field_ != field.velocity_polygon_name_) { - monitored_next_field_ = field.velocity_polygon_name_; - next_field_free_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); - } - - if (!currently_free) { - next_field_free_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); - return false; - } - - // Field is currently free — check if it has been free long enough - if (next_field_free_stamp_.nanoseconds() == 0) { - next_field_free_stamp_ = clock_->now(); - } - return (clock_->now() - next_field_free_stamp_) >= - rclcpp::Duration::from_seconds(field_free_duration_); -} - bool VelocityPolygon::validateSteering( const Velocity & cmd_vel_in, const Velocity & odom_vel, @@ -607,92 +555,19 @@ bool VelocityPolygon::validateSteering( target_steering_angle <= current_field->steering_angle_max_; debug_msg.same_bucket = same_bucket; - // When the robot is at near-standstill or its speed falls in a gap between - // fields (current_field == nullptr), use the TARGET direction's fields to - // check for obstacles. This prevents blind acceleration from low speed into - // obstructed fields that would otherwise trigger an e-stop. - bool low_speed = current_field == nullptr || - std::abs(current_sw_speed) < low_speed_threshold_; - - if (low_speed) { - bool target_forward = target_sw_speed >= 0; - auto target_fields = findFieldsForAngle(target_steering_angle, target_forward); - - if (!target_fields.empty()) { - // Use the slowest field (index 0) as effective "current" and check one - // step up, mirroring the normal progressive-acceleration logic. - const SubPolygonParameter * limit_field = target_fields[0]; - - int slowest_pts = getPointsInsideSubPolygon(*target_fields[0], collision_points_map); - debug_msg.next_field_collision_pts = slowest_pts; - - if (slowest_pts >= min_points_ && !isCreepingField(*target_fields[0])) { - // Slowest non-creeping field has obstacles — limit to creeping speed - // so the robot can always maneuver slowly, even when the way ahead - // is blocked at higher speeds. - double creep_sw = low_speed_threshold_; - double sign = target_forward ? 1.0 : -1.0; - double result_sw = baselinkToSteeringSpeed(result_vel.x, result_vel.tw); - if (std::abs(result_sw) > creep_sw) { - result_vel.x = sign * creep_sw * std::cos(target_steering_angle); - result_vel.tw = sign * creep_sw * std::sin(target_steering_angle) / wheelbase_; - debug_msg.speed_limit_applied = result_vel.x; - modified = true; - } - } else { - // Slowest field is clear (or is a creeping field — always allowed). - // Check one step up using the stable-free timer to prevent oscillation. - bool next_occupied = false; - if (target_fields.size() > 1) { - debug_msg.next_field_name = target_fields[1]->velocity_polygon_name_; - int next_pts; - bool stably_free = isNextFieldStablyFree( - *target_fields[1], collision_points_map, next_pts); - debug_msg.next_field_collision_pts = next_pts; - if (stably_free) { - limit_field = target_fields[1]; - } else { - next_occupied = true; - } - } - - double limit_sw = target_forward ? - limit_field->linear_max_ : limit_field->linear_min_; - - // When the next field is occupied, use linear_limit (safety speed) - if (next_occupied) { - double limit_cap = std::abs(limit_field->linear_limit_); - if (limit_cap > 0.0 && limit_cap < std::abs(limit_sw)) { - limit_sw = target_forward ? limit_cap : -limit_cap; - } - } - double result_sw_speed = baselinkToSteeringSpeed(result_vel.x, result_vel.tw); - if (std::abs(result_sw_speed) > std::abs(limit_sw)) { - double limited_x = limit_sw * std::cos(target_steering_angle); - double limited_tw = limit_sw * std::sin(target_steering_angle) / wheelbase_; - debug_msg.speed_limit_applied = limited_x; - result_vel.x = limited_x; - result_vel.tw = limited_tw; - modified = true; - } - } - - if (modified) { - robot_action.req_vel = result_vel; - robot_action.polygon_name = polygon_name_; - robot_action.action_type = LIMIT; - } + if (same_bucket) { + // Skip same-bucket limiting when the robot is at or near standstill. + // The current field at standstill (e.g. creeping/stopped) is not meaningful + // for speed limiting — the robot must be free to start moving. + if (std::abs(current_sw_speed) < low_speed_threshold_) { + debug_msg.modified = false; + debug_msg.result_vel_x = result_vel.x; + debug_msg.result_vel_y = result_vel.y; + debug_msg.result_vel_tw = result_vel.tw; + steering_debug_pub_->publish(debug_msg); + return false; } - debug_msg.modified = modified; - debug_msg.result_vel_x = result_vel.x; - debug_msg.result_vel_y = result_vel.y; - debug_msg.result_vel_tw = result_vel.tw; - steering_debug_pub_->publish(debug_msg); - return modified; - } - - if (same_bucket) { // Check one field up from the current field at the current physical angle. // Always limit speed to at most the next reachable field's max: // - Next field exists and is collision-free → limit to next field's max @@ -703,7 +578,6 @@ bool VelocityPolygon::validateSteering( bool forward = current_sw_speed >= 0; auto fields_at_angle = findFieldsForAngle(current_sa, forward); const SubPolygonParameter * limit_field = current_field; - bool next_field_occupied = false; // Find the current field in the sorted list for (size_t i = 0; i < fields_at_angle.size(); i++) { @@ -714,16 +588,13 @@ bool VelocityPolygon::validateSteering( if (i + 1 < fields_at_angle.size()) { const SubPolygonParameter * next_field = fields_at_angle[i + 1]; debug_msg.next_field_name = next_field->velocity_polygon_name_; - int pts; - bool stably_free = isNextFieldStablyFree( - *next_field, collision_points_map, pts); + int pts = getPointsInsideSubPolygon(*next_field, collision_points_map); debug_msg.next_field_collision_pts = pts; - if (stably_free) { - // Next field has been stably collision-free — allow up to next field's max + if (pts < min_points_) { + // Next field is collision-free — allow up to next field's max limit_field = next_field; - } else { - next_field_occupied = true; } + // else: next field has obstacles — stay at current field's max } // else: no faster field — stay at current field's max break; @@ -731,17 +602,6 @@ bool VelocityPolygon::validateSteering( double limit_sw = forward ? limit_field->linear_max_ : limit_field->linear_min_; - - // When the next field is occupied, use the current field's linear_limit - // (safety speed) instead of its full range boundary. Obstacles are clearly - // nearby if the next field has collision points, so the more conservative - // limit is appropriate even when the current polygon doesn't detect them. - if (next_field_occupied) { - double limit_cap = std::abs(current_field->linear_limit_); - if (limit_cap > 0.0 && limit_cap < std::abs(limit_sw)) { - limit_sw = forward ? limit_cap : -limit_cap; - } - } double result_sw_speed = baselinkToSteeringSpeed(result_vel.x, result_vel.tw); if (std::abs(result_sw_speed) > std::abs(limit_sw)) { double limited_x = limit_sw * std::cos(target_steering_angle); @@ -767,7 +627,23 @@ bool VelocityPolygon::validateSteering( } // 3. Different bucket — find neighbouring bucket (one step in steering direction) - // current_field is guaranteed non-null here (low_speed case handled above) + if (current_field == nullptr) { + RCLCPP_WARN( + logger_, + "[%s] validateSteering: current_field is null — no field matches current velocity. " + "odom_vel=(%.3f, %.3f, %.3f), current_sw_speed=%.3f, current_sa=%.3f, " + "cmd_vel=(%.3f, %.3f, %.3f), target_sw_speed=%.3f, target_sa=%.3f. " + "Check that velocity polygon field ranges cover all reachable velocities.", + polygon_name_.c_str(), + odom_vel.x, odom_vel.y, odom_vel.tw, current_sw_speed, current_sa, + cmd_vel_in.x, cmd_vel_in.y, cmd_vel_in.tw, target_sw_speed, target_steering_angle); + debug_msg.modified = false; + debug_msg.result_vel_x = result_vel.x; + debug_msg.result_vel_y = result_vel.y; + debug_msg.result_vel_tw = result_vel.tw; + steering_debug_pub_->publish(debug_msg); + return false; + } double neighbour_angle; if (target_steering_angle > current_sa) { @@ -792,20 +668,16 @@ bool VelocityPolygon::validateSteering( } // Find the fastest field that covers max(|current|, |target|) speed - // Use signed speed so the range check works for both forward and backward fields - // (backward fields have linear_min_ < linear_max_ < 0, so |min| > |max| - // which inverts the abs-based comparison) double max_sw_speed = std::max(std::abs(current_sw_speed), std::abs(target_sw_speed)); - double signed_max_sw = forward ? max_sw_speed : -max_sw_speed; // Find the valid field: start from fastest covering field, walk down const SubPolygonParameter * valid_field = nullptr; int start_idx = static_cast(neighbour_fields.size()) - 1; - // Find the starting field (fastest that covers the speed) + // Find the starting field (fastest that covers max_sw_speed) for (int i = start_idx; i >= 0; i--) { - if (signed_max_sw >= neighbour_fields[i]->linear_min_ && - signed_max_sw <= neighbour_fields[i]->linear_max_) + if (max_sw_speed >= std::abs(neighbour_fields[i]->linear_min_) && + max_sw_speed <= std::abs(neighbour_fields[i]->linear_max_)) { start_idx = i; break; From 5b777545b4f2503b7bcb65eae5b7701bc275db87 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Tue, 31 Mar 2026 12:21:04 +0200 Subject: [PATCH 27/35] Implemented potential fix for backwards driving. --- .../src/velocity_polygon.cpp | 47 +++++++++++++------ 1 file changed, 33 insertions(+), 14 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 4c8912fe3b7..1926c54b58f 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -296,9 +296,14 @@ bool VelocityPolygon::isInRange( return false; } - // Check steering angle range - in_range &= current_steering_angle_ <= sub_polygon.steering_angle_max_ && - current_steering_angle_ >= sub_polygon.steering_angle_min_; + // Check steering angle range. + // When driving backward, negate the angle for field lookup: the swept area + // flips relative to the steering angle, so e.g. backward + far-left steering + // needs the far-right polygon shape (which covers where the rear actually heads). + double lookup_angle = (cmd_vel_in.x < 0) ? + -current_steering_angle_ : current_steering_angle_; + in_range &= lookup_angle <= sub_polygon.steering_angle_max_ && + lookup_angle >= sub_polygon.steering_angle_min_; return in_range; } @@ -545,14 +550,21 @@ bool VelocityPolygon::validateSteering( return false; } - const SubPolygonParameter * current_field = findField(current_sw_speed, current_sa); + // For field lookup, negate the steering angle when driving backward: + // the swept area flips, so backward + far-left needs the far-right polygon. + // Keep the physical angles for speed/tw calculations (cos, sin, tan). + bool driving_backward = odom_vel.x < 0; + double current_lookup_sa = driving_backward ? -current_sa : current_sa; + double target_lookup_sa = driving_backward ? -target_steering_angle : target_steering_angle; + + const SubPolygonParameter * current_field = findField(current_sw_speed, current_lookup_sa); debug_msg.current_field_name = current_field ? current_field->velocity_polygon_name_ : ""; // 2. Check if target angle is in same bucket (angle range) as current bool same_bucket = current_field != nullptr && - target_steering_angle >= current_field->steering_angle_min_ && - target_steering_angle <= current_field->steering_angle_max_; + target_lookup_sa >= current_field->steering_angle_min_ && + target_lookup_sa <= current_field->steering_angle_max_; debug_msg.same_bucket = same_bucket; if (same_bucket) { @@ -576,7 +588,7 @@ bool VelocityPolygon::validateSteering( // This ensures progressive speed increase (one field per cycle) and // prevents exceeding defined field ranges into e-stop territory. bool forward = current_sw_speed >= 0; - auto fields_at_angle = findFieldsForAngle(current_sa, forward); + auto fields_at_angle = findFieldsForAngle(current_lookup_sa, forward); const SubPolygonParameter * limit_field = current_field; // Find the current field in the sorted list @@ -646,14 +658,14 @@ bool VelocityPolygon::validateSteering( } double neighbour_angle; - if (target_steering_angle > current_sa) { + if (target_lookup_sa > current_lookup_sa) { neighbour_angle = current_field->steering_angle_max_; } else { neighbour_angle = current_field->steering_angle_min_; } // Step just past the boundary to land in the neighbouring bucket constexpr double kAngleEps = 0.01; - double lookup_angle = (target_steering_angle > current_sa) ? + double lookup_angle = (target_lookup_sa > current_lookup_sa) ? neighbour_angle + kAngleEps : neighbour_angle - kAngleEps; bool forward = target_sw_speed >= 0; @@ -719,8 +731,10 @@ bool VelocityPolygon::validateSteering( // Forward: limit to linear_max; Backward: limit to linear_min double valid_limit_sw = (target_sw_speed >= 0) ? valid_field->linear_max_ : valid_field->linear_min_; + // Convert field boundary angle back to physical for speed calculation + double physical_neighbour_angle = driving_backward ? -neighbour_angle : neighbour_angle; double valid_max_baselink = steeringToBaselinkSpeed( - valid_limit_sw, neighbour_angle); + valid_limit_sw, physical_neighbour_angle); if (std::abs(result_vel.x) > std::abs(valid_max_baselink)) { debug_msg.speed_limit_applied = valid_max_baselink; result_vel.x = valid_max_baselink; @@ -732,13 +746,15 @@ bool VelocityPolygon::validateSteering( double valid_max_baselink_abs = std::abs(valid_max_baselink); if (current_baselink_abs > valid_max_baselink_abs && current_field != nullptr) { double limited_sa; - if (target_steering_angle > current_sa) { + if (target_lookup_sa > current_lookup_sa) { limited_sa = current_field->steering_angle_max_; } else { limited_sa = current_field->steering_angle_min_; } debug_msg.steering_angle_limit = limited_sa; - result_vel.tw = steeringAngleToTw(result_vel.x, limited_sa); + // Convert field boundary back to physical angle for tw calculation + double physical_limited_sa = driving_backward ? -limited_sa : limited_sa; + result_vel.tw = steeringAngleToTw(result_vel.x, physical_limited_sa); modified = true; } @@ -771,8 +787,11 @@ bool VelocityPolygon::clampToMaxField( double cmd_x = robot_action.req_vel.x; bool forward = cmd_x >= 0; - // Find all fields at the physical steering angle for the commanded direction - auto fields = findFieldsForAngle(physical_sa, forward); + // Negate the angle for field lookup when driving backward + double clamp_lookup_sa = (odom_vel.x < 0) ? -physical_sa : physical_sa; + + // Find all fields at the steering angle for the commanded direction + auto fields = findFieldsForAngle(clamp_lookup_sa, forward); if (fields.empty()) { // No fields at this angle — if velocity is non-zero, zero it From 2431cbeb373fa1dbe7276782285bf57aac608e37 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Tue, 31 Mar 2026 13:34:27 +0200 Subject: [PATCH 28/35] Revert "Implemented potential fix for backwards driving." This reverts commit 5b777545b4f2503b7bcb65eae5b7701bc275db87. --- .../src/velocity_polygon.cpp | 47 ++++++------------- 1 file changed, 14 insertions(+), 33 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 1926c54b58f..4c8912fe3b7 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -296,14 +296,9 @@ bool VelocityPolygon::isInRange( return false; } - // Check steering angle range. - // When driving backward, negate the angle for field lookup: the swept area - // flips relative to the steering angle, so e.g. backward + far-left steering - // needs the far-right polygon shape (which covers where the rear actually heads). - double lookup_angle = (cmd_vel_in.x < 0) ? - -current_steering_angle_ : current_steering_angle_; - in_range &= lookup_angle <= sub_polygon.steering_angle_max_ && - lookup_angle >= sub_polygon.steering_angle_min_; + // Check steering angle range + in_range &= current_steering_angle_ <= sub_polygon.steering_angle_max_ && + current_steering_angle_ >= sub_polygon.steering_angle_min_; return in_range; } @@ -550,21 +545,14 @@ bool VelocityPolygon::validateSteering( return false; } - // For field lookup, negate the steering angle when driving backward: - // the swept area flips, so backward + far-left needs the far-right polygon. - // Keep the physical angles for speed/tw calculations (cos, sin, tan). - bool driving_backward = odom_vel.x < 0; - double current_lookup_sa = driving_backward ? -current_sa : current_sa; - double target_lookup_sa = driving_backward ? -target_steering_angle : target_steering_angle; - - const SubPolygonParameter * current_field = findField(current_sw_speed, current_lookup_sa); + const SubPolygonParameter * current_field = findField(current_sw_speed, current_sa); debug_msg.current_field_name = current_field ? current_field->velocity_polygon_name_ : ""; // 2. Check if target angle is in same bucket (angle range) as current bool same_bucket = current_field != nullptr && - target_lookup_sa >= current_field->steering_angle_min_ && - target_lookup_sa <= current_field->steering_angle_max_; + target_steering_angle >= current_field->steering_angle_min_ && + target_steering_angle <= current_field->steering_angle_max_; debug_msg.same_bucket = same_bucket; if (same_bucket) { @@ -588,7 +576,7 @@ bool VelocityPolygon::validateSteering( // This ensures progressive speed increase (one field per cycle) and // prevents exceeding defined field ranges into e-stop territory. bool forward = current_sw_speed >= 0; - auto fields_at_angle = findFieldsForAngle(current_lookup_sa, forward); + auto fields_at_angle = findFieldsForAngle(current_sa, forward); const SubPolygonParameter * limit_field = current_field; // Find the current field in the sorted list @@ -658,14 +646,14 @@ bool VelocityPolygon::validateSteering( } double neighbour_angle; - if (target_lookup_sa > current_lookup_sa) { + if (target_steering_angle > current_sa) { neighbour_angle = current_field->steering_angle_max_; } else { neighbour_angle = current_field->steering_angle_min_; } // Step just past the boundary to land in the neighbouring bucket constexpr double kAngleEps = 0.01; - double lookup_angle = (target_lookup_sa > current_lookup_sa) ? + double lookup_angle = (target_steering_angle > current_sa) ? neighbour_angle + kAngleEps : neighbour_angle - kAngleEps; bool forward = target_sw_speed >= 0; @@ -731,10 +719,8 @@ bool VelocityPolygon::validateSteering( // Forward: limit to linear_max; Backward: limit to linear_min double valid_limit_sw = (target_sw_speed >= 0) ? valid_field->linear_max_ : valid_field->linear_min_; - // Convert field boundary angle back to physical for speed calculation - double physical_neighbour_angle = driving_backward ? -neighbour_angle : neighbour_angle; double valid_max_baselink = steeringToBaselinkSpeed( - valid_limit_sw, physical_neighbour_angle); + valid_limit_sw, neighbour_angle); if (std::abs(result_vel.x) > std::abs(valid_max_baselink)) { debug_msg.speed_limit_applied = valid_max_baselink; result_vel.x = valid_max_baselink; @@ -746,15 +732,13 @@ bool VelocityPolygon::validateSteering( double valid_max_baselink_abs = std::abs(valid_max_baselink); if (current_baselink_abs > valid_max_baselink_abs && current_field != nullptr) { double limited_sa; - if (target_lookup_sa > current_lookup_sa) { + if (target_steering_angle > current_sa) { limited_sa = current_field->steering_angle_max_; } else { limited_sa = current_field->steering_angle_min_; } debug_msg.steering_angle_limit = limited_sa; - // Convert field boundary back to physical angle for tw calculation - double physical_limited_sa = driving_backward ? -limited_sa : limited_sa; - result_vel.tw = steeringAngleToTw(result_vel.x, physical_limited_sa); + result_vel.tw = steeringAngleToTw(result_vel.x, limited_sa); modified = true; } @@ -787,11 +771,8 @@ bool VelocityPolygon::clampToMaxField( double cmd_x = robot_action.req_vel.x; bool forward = cmd_x >= 0; - // Negate the angle for field lookup when driving backward - double clamp_lookup_sa = (odom_vel.x < 0) ? -physical_sa : physical_sa; - - // Find all fields at the steering angle for the commanded direction - auto fields = findFieldsForAngle(clamp_lookup_sa, forward); + // Find all fields at the physical steering angle for the commanded direction + auto fields = findFieldsForAngle(physical_sa, forward); if (fields.empty()) { // No fields at this angle — if velocity is non-zero, zero it From 4c40772d0f7183d5a8fc06f152c1090c922d5c7b Mon Sep 17 00:00:00 2001 From: juuulianx Date: Tue, 31 Mar 2026 13:54:24 +0200 Subject: [PATCH 29/35] Revert "Revert "Implemented potential fix for backwards driving."" This reverts commit 2431cbeb373fa1dbe7276782285bf57aac608e37. --- .../src/velocity_polygon.cpp | 47 +++++++++++++------ 1 file changed, 33 insertions(+), 14 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 4c8912fe3b7..1926c54b58f 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -296,9 +296,14 @@ bool VelocityPolygon::isInRange( return false; } - // Check steering angle range - in_range &= current_steering_angle_ <= sub_polygon.steering_angle_max_ && - current_steering_angle_ >= sub_polygon.steering_angle_min_; + // Check steering angle range. + // When driving backward, negate the angle for field lookup: the swept area + // flips relative to the steering angle, so e.g. backward + far-left steering + // needs the far-right polygon shape (which covers where the rear actually heads). + double lookup_angle = (cmd_vel_in.x < 0) ? + -current_steering_angle_ : current_steering_angle_; + in_range &= lookup_angle <= sub_polygon.steering_angle_max_ && + lookup_angle >= sub_polygon.steering_angle_min_; return in_range; } @@ -545,14 +550,21 @@ bool VelocityPolygon::validateSteering( return false; } - const SubPolygonParameter * current_field = findField(current_sw_speed, current_sa); + // For field lookup, negate the steering angle when driving backward: + // the swept area flips, so backward + far-left needs the far-right polygon. + // Keep the physical angles for speed/tw calculations (cos, sin, tan). + bool driving_backward = odom_vel.x < 0; + double current_lookup_sa = driving_backward ? -current_sa : current_sa; + double target_lookup_sa = driving_backward ? -target_steering_angle : target_steering_angle; + + const SubPolygonParameter * current_field = findField(current_sw_speed, current_lookup_sa); debug_msg.current_field_name = current_field ? current_field->velocity_polygon_name_ : ""; // 2. Check if target angle is in same bucket (angle range) as current bool same_bucket = current_field != nullptr && - target_steering_angle >= current_field->steering_angle_min_ && - target_steering_angle <= current_field->steering_angle_max_; + target_lookup_sa >= current_field->steering_angle_min_ && + target_lookup_sa <= current_field->steering_angle_max_; debug_msg.same_bucket = same_bucket; if (same_bucket) { @@ -576,7 +588,7 @@ bool VelocityPolygon::validateSteering( // This ensures progressive speed increase (one field per cycle) and // prevents exceeding defined field ranges into e-stop territory. bool forward = current_sw_speed >= 0; - auto fields_at_angle = findFieldsForAngle(current_sa, forward); + auto fields_at_angle = findFieldsForAngle(current_lookup_sa, forward); const SubPolygonParameter * limit_field = current_field; // Find the current field in the sorted list @@ -646,14 +658,14 @@ bool VelocityPolygon::validateSteering( } double neighbour_angle; - if (target_steering_angle > current_sa) { + if (target_lookup_sa > current_lookup_sa) { neighbour_angle = current_field->steering_angle_max_; } else { neighbour_angle = current_field->steering_angle_min_; } // Step just past the boundary to land in the neighbouring bucket constexpr double kAngleEps = 0.01; - double lookup_angle = (target_steering_angle > current_sa) ? + double lookup_angle = (target_lookup_sa > current_lookup_sa) ? neighbour_angle + kAngleEps : neighbour_angle - kAngleEps; bool forward = target_sw_speed >= 0; @@ -719,8 +731,10 @@ bool VelocityPolygon::validateSteering( // Forward: limit to linear_max; Backward: limit to linear_min double valid_limit_sw = (target_sw_speed >= 0) ? valid_field->linear_max_ : valid_field->linear_min_; + // Convert field boundary angle back to physical for speed calculation + double physical_neighbour_angle = driving_backward ? -neighbour_angle : neighbour_angle; double valid_max_baselink = steeringToBaselinkSpeed( - valid_limit_sw, neighbour_angle); + valid_limit_sw, physical_neighbour_angle); if (std::abs(result_vel.x) > std::abs(valid_max_baselink)) { debug_msg.speed_limit_applied = valid_max_baselink; result_vel.x = valid_max_baselink; @@ -732,13 +746,15 @@ bool VelocityPolygon::validateSteering( double valid_max_baselink_abs = std::abs(valid_max_baselink); if (current_baselink_abs > valid_max_baselink_abs && current_field != nullptr) { double limited_sa; - if (target_steering_angle > current_sa) { + if (target_lookup_sa > current_lookup_sa) { limited_sa = current_field->steering_angle_max_; } else { limited_sa = current_field->steering_angle_min_; } debug_msg.steering_angle_limit = limited_sa; - result_vel.tw = steeringAngleToTw(result_vel.x, limited_sa); + // Convert field boundary back to physical angle for tw calculation + double physical_limited_sa = driving_backward ? -limited_sa : limited_sa; + result_vel.tw = steeringAngleToTw(result_vel.x, physical_limited_sa); modified = true; } @@ -771,8 +787,11 @@ bool VelocityPolygon::clampToMaxField( double cmd_x = robot_action.req_vel.x; bool forward = cmd_x >= 0; - // Find all fields at the physical steering angle for the commanded direction - auto fields = findFieldsForAngle(physical_sa, forward); + // Negate the angle for field lookup when driving backward + double clamp_lookup_sa = (odom_vel.x < 0) ? -physical_sa : physical_sa; + + // Find all fields at the steering angle for the commanded direction + auto fields = findFieldsForAngle(clamp_lookup_sa, forward); if (fields.empty()) { // No fields at this angle — if velocity is non-zero, zero it From 5c2e3a56f6d8591ed9e79c4bf68768cb49eb88bb Mon Sep 17 00:00:00 2001 From: Johannes Plapp Date: Tue, 31 Mar 2026 16:16:49 +0200 Subject: [PATCH 30/35] clarify readme --- nav2_collision_monitor/README.md | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index ba33a3b463c..8b22095b7c3 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -140,26 +140,31 @@ If the obstacle e.g. moves with us or is on the side, we can keep that speed. ### 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 speed) > low threshold → keep steering angle (we must anyway just slow down asap) -* if abs(current speed) < low threshold → allow steering +* 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) and abs(current speed) are < low threshold. If yes → done +1. check if both abs(target steering wheel speed) and abs(current steering wheel speed) are < low threshold. If yes → done 2. check if target angle is in same bucket as current angle. If yes → - 1. if the result velocity falls into the some faster field (same bucket), check the subsequent field for collision. Only the one-step faster field needs to be checked, even if target speed is in a much faster field. if in collision, limit speed to current field → then done - 2. 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. -3. in the neighbouring bucket, determine max speed / valid field + 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. check the one-step faster field in the same bucket for collision. 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 → limit speed to next field's max. If the next field has obstacles or no faster field exists → limit speed to current field's max. → then done +3. 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. +4. 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) -4. now adapt speed and steering angle +5. now adapt speed and steering angle 1. limit target speed to max speed of valid field 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 4b, the current speed will not be above max valid speed +After some iterations, the current speed will be in the valid field → AMR will be allowed to steer, as in 5b, the current speed will not be above max valid speed ### Field exceedance prevention @@ -171,6 +176,8 @@ For any given steering angle, the lidar has a finite set of configured fields (e 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. From 8a778b08c2d61ba705077a60c7b50e104cb7ef54 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Wed, 1 Apr 2026 10:58:09 +0200 Subject: [PATCH 31/35] Reverted angle inversion logic. --- .../src/velocity_polygon.cpp | 45 ++++++------------- 1 file changed, 14 insertions(+), 31 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 1926c54b58f..d5dc48b5884 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -296,14 +296,9 @@ bool VelocityPolygon::isInRange( return false; } - // Check steering angle range. - // When driving backward, negate the angle for field lookup: the swept area - // flips relative to the steering angle, so e.g. backward + far-left steering - // needs the far-right polygon shape (which covers where the rear actually heads). - double lookup_angle = (cmd_vel_in.x < 0) ? - -current_steering_angle_ : current_steering_angle_; - in_range &= lookup_angle <= sub_polygon.steering_angle_max_ && - lookup_angle >= sub_polygon.steering_angle_min_; + // Check steering angle range + in_range &= current_steering_angle_ <= sub_polygon.steering_angle_max_ && + current_steering_angle_ >= sub_polygon.steering_angle_min_; return in_range; } @@ -552,19 +547,14 @@ bool VelocityPolygon::validateSteering( // For field lookup, negate the steering angle when driving backward: // the swept area flips, so backward + far-left needs the far-right polygon. - // Keep the physical angles for speed/tw calculations (cos, sin, tan). - bool driving_backward = odom_vel.x < 0; - double current_lookup_sa = driving_backward ? -current_sa : current_sa; - double target_lookup_sa = driving_backward ? -target_steering_angle : target_steering_angle; - - const SubPolygonParameter * current_field = findField(current_sw_speed, current_lookup_sa); + const SubPolygonParameter * current_field = findField(current_sw_speed, current_sa); debug_msg.current_field_name = current_field ? current_field->velocity_polygon_name_ : ""; // 2. Check if target angle is in same bucket (angle range) as current bool same_bucket = current_field != nullptr && - target_lookup_sa >= current_field->steering_angle_min_ && - target_lookup_sa <= current_field->steering_angle_max_; + target_steering_angle >= current_field->steering_angle_min_ && + target_steering_angle <= current_field->steering_angle_max_; debug_msg.same_bucket = same_bucket; if (same_bucket) { @@ -588,7 +578,7 @@ bool VelocityPolygon::validateSteering( // This ensures progressive speed increase (one field per cycle) and // prevents exceeding defined field ranges into e-stop territory. bool forward = current_sw_speed >= 0; - auto fields_at_angle = findFieldsForAngle(current_lookup_sa, forward); + auto fields_at_angle = findFieldsForAngle(current_sa, forward); const SubPolygonParameter * limit_field = current_field; // Find the current field in the sorted list @@ -658,14 +648,14 @@ bool VelocityPolygon::validateSteering( } double neighbour_angle; - if (target_lookup_sa > current_lookup_sa) { + if (target_steering_angle > current_sa) { neighbour_angle = current_field->steering_angle_max_; } else { neighbour_angle = current_field->steering_angle_min_; } // Step just past the boundary to land in the neighbouring bucket constexpr double kAngleEps = 0.01; - double lookup_angle = (target_lookup_sa > current_lookup_sa) ? + double lookup_angle = (target_steering_angle > current_sa) ? neighbour_angle + kAngleEps : neighbour_angle - kAngleEps; bool forward = target_sw_speed >= 0; @@ -731,10 +721,8 @@ bool VelocityPolygon::validateSteering( // Forward: limit to linear_max; Backward: limit to linear_min double valid_limit_sw = (target_sw_speed >= 0) ? valid_field->linear_max_ : valid_field->linear_min_; - // Convert field boundary angle back to physical for speed calculation - double physical_neighbour_angle = driving_backward ? -neighbour_angle : neighbour_angle; double valid_max_baselink = steeringToBaselinkSpeed( - valid_limit_sw, physical_neighbour_angle); + valid_limit_sw, neighbour_angle); if (std::abs(result_vel.x) > std::abs(valid_max_baselink)) { debug_msg.speed_limit_applied = valid_max_baselink; result_vel.x = valid_max_baselink; @@ -746,15 +734,13 @@ bool VelocityPolygon::validateSteering( double valid_max_baselink_abs = std::abs(valid_max_baselink); if (current_baselink_abs > valid_max_baselink_abs && current_field != nullptr) { double limited_sa; - if (target_lookup_sa > current_lookup_sa) { + if (target_steering_angle > current_sa) { limited_sa = current_field->steering_angle_max_; } else { limited_sa = current_field->steering_angle_min_; } debug_msg.steering_angle_limit = limited_sa; - // Convert field boundary back to physical angle for tw calculation - double physical_limited_sa = driving_backward ? -limited_sa : limited_sa; - result_vel.tw = steeringAngleToTw(result_vel.x, physical_limited_sa); + result_vel.tw = steeringAngleToTw(result_vel.x, limited_sa); modified = true; } @@ -787,11 +773,8 @@ bool VelocityPolygon::clampToMaxField( double cmd_x = robot_action.req_vel.x; bool forward = cmd_x >= 0; - // Negate the angle for field lookup when driving backward - double clamp_lookup_sa = (odom_vel.x < 0) ? -physical_sa : physical_sa; - - // Find all fields at the steering angle for the commanded direction - auto fields = findFieldsForAngle(clamp_lookup_sa, forward); + // Find all fields at the physical steering angle for the commanded direction + auto fields = findFieldsForAngle(physical_sa, forward); if (fields.empty()) { // No fields at this angle — if velocity is non-zero, zero it From 24596c165e3a01994c1ba19399d3d4102e424d47 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Wed, 1 Apr 2026 15:04:35 +0200 Subject: [PATCH 32/35] Fix: when limiting speed, also set tw to preserve the target steering angle. --- nav2_collision_monitor/src/velocity_polygon.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index d5dc48b5884..b98165a6c70 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -726,6 +726,10 @@ bool VelocityPolygon::validateSteering( if (std::abs(result_vel.x) > std::abs(valid_max_baselink)) { debug_msg.speed_limit_applied = valid_max_baselink; result_vel.x = valid_max_baselink; + // Also adjust tw to preserve the target steering angle, otherwise + // reducing x while keeping tw creates an absurd steering angle + // that falls outside all configured field buckets. + result_vel.tw = steeringAngleToTw(result_vel.x, target_steering_angle); modified = true; } From 4b15750289e2a978695cbac32e323e7078ca5218 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Wed, 1 Apr 2026 15:42:33 +0200 Subject: [PATCH 33/35] Fix: when limiting speed, also set tw to preserve the target steering angle, properly this time. --- nav2_collision_monitor/src/velocity_polygon.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index b98165a6c70..76b8dd652b5 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -726,10 +726,12 @@ bool VelocityPolygon::validateSteering( if (std::abs(result_vel.x) > std::abs(valid_max_baselink)) { debug_msg.speed_limit_applied = valid_max_baselink; result_vel.x = valid_max_baselink; - // Also adjust tw to preserve the target steering angle, otherwise + // Also adjust tw to match the neighbour bucket boundary angle, otherwise // reducing x while keeping tw creates an absurd steering angle // that falls outside all configured field buckets. - result_vel.tw = steeringAngleToTw(result_vel.x, target_steering_angle); + // Use neighbour_angle (the boundary toward the target bucket) rather than + // target_steering_angle, which may overshoot the valid field's bucket. + result_vel.tw = steeringAngleToTw(result_vel.x, neighbour_angle); modified = true; } From a122559dfbeab4e0a916422dd2e85c7e169ef271 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Wed, 1 Apr 2026 17:21:30 +0200 Subject: [PATCH 34/35] Added debug polygon of next_polygon. --- .../velocity_polygon.hpp | 2 + .../src/velocity_polygon.cpp | 38 +++++++++++++++++++ 2 files changed, 40 insertions(+) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp index a405740f53f..bba1ca06a67 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp @@ -219,6 +219,8 @@ class VelocityPolygon : public Polygon rclcpp::Clock::SharedPtr clock_; // Debug publisher for steering validation rclcpp::Publisher::SharedPtr steering_debug_pub_; + // Publisher for the next/valid field polygon being checked for obstacles + rclcpp::Publisher::SharedPtr next_field_poly_pub_; // Current subpolygon name std::string current_subpolygon_name_; // Variables diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 76b8dd652b5..76e5e17ba0b 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -220,6 +220,9 @@ bool VelocityPolygon::getParameters( steering_debug_pub_ = node->create_publisher( "~/steering_validation_debug", rclcpp::QoS(1)); + next_field_poly_pub_ = node->create_publisher( + "~/next_field_polygon", rclcpp::QoS(1)); + return true; } @@ -459,6 +462,9 @@ bool VelocityPolygon::validateSteering( return false; } + // Track the field being checked for obstacles (published for visualization) + const SubPolygonParameter * checked_field = nullptr; + nav2_msgs::msg::SteeringValidationDebug debug_msg; debug_msg.header.stamp = clock_->now(); debug_msg.polygon_name = polygon_name_; @@ -589,6 +595,7 @@ bool VelocityPolygon::validateSteering( if (i + 1 < fields_at_angle.size()) { const SubPolygonParameter * next_field = fields_at_angle[i + 1]; + checked_field = next_field; debug_msg.next_field_name = next_field->velocity_polygon_name_; int pts = getPointsInsideSubPolygon(*next_field, collision_points_map); debug_msg.next_field_collision_pts = pts; @@ -625,6 +632,21 @@ bool VelocityPolygon::validateSteering( debug_msg.result_vel_y = result_vel.y; debug_msg.result_vel_tw = result_vel.tw; steering_debug_pub_->publish(debug_msg); + + // Publish the next field polygon for 3D visualization + if (checked_field != nullptr && next_field_poly_pub_->get_subscription_count() > 0) { + geometry_msgs::msg::PolygonStamped poly_msg; + poly_msg.header.frame_id = base_frame_id_; + poly_msg.header.stamp = clock_->now(); + for (const auto & p : checked_field->poly_) { + geometry_msgs::msg::Point32 pt; + pt.x = p.x; + pt.y = p.y; + poly_msg.polygon.points.push_back(pt); + } + next_field_poly_pub_->publish(poly_msg); + } + return modified; } @@ -713,6 +735,7 @@ bool VelocityPolygon::validateSteering( // (allowed even if in collision) valid_field = neighbour_fields[0]; // sorted ascending, index 0 is slowest } + checked_field = valid_field; } debug_msg.valid_field_name = valid_field->velocity_polygon_name_; @@ -761,6 +784,21 @@ bool VelocityPolygon::validateSteering( debug_msg.result_vel_y = result_vel.y; debug_msg.result_vel_tw = result_vel.tw; steering_debug_pub_->publish(debug_msg); + + // Publish the next/valid field polygon for 3D visualization + if (checked_field != nullptr && next_field_poly_pub_->get_subscription_count() > 0) { + geometry_msgs::msg::PolygonStamped poly_msg; + poly_msg.header.frame_id = base_frame_id_; + poly_msg.header.stamp = clock_->now(); + for (const auto & p : checked_field->poly_) { + geometry_msgs::msg::Point32 pt; + pt.x = p.x; + pt.y = p.y; + poly_msg.polygon.points.push_back(pt); + } + next_field_poly_pub_->publish(poly_msg); + } + return modified; } From bdd8adf9b0b2ab845ae130c72fb88d34d574a5f9 Mon Sep 17 00:00:00 2001 From: juuulianx Date: Thu, 2 Apr 2026 16:26:15 +0200 Subject: [PATCH 35/35] Fixed logic error in collision monitor. --- nav2_collision_monitor/README.md | 15 +- .../src/velocity_polygon.cpp | 143 ++++++++------- .../test/velocity_polygons_test.cpp | 172 ++++++++++++++++++ 3 files changed, 254 insertions(+), 76 deletions(-) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 8b22095b7c3..cb36b0b2925 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -152,19 +152,20 @@ If speed goes through zero (so sign(target speed) <> sign(current speed)): else: 1. check if both abs(target steering wheel speed) and abs(current steering wheel speed) are < low threshold. If yes → done -2. check if target angle is in same bucket as current angle. If yes → +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. check the one-step faster field in the same bucket for collision. 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 → limit speed to next field's max. If the next field has obstacles or no faster field exists → limit speed to current field's max. → then done -3. 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. -4. in the neighbouring bucket, determine max speed / valid field + 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) -5. now adapt speed and steering angle - 1. limit target speed to max speed of valid field +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 5b, the current speed will not be above max valid speed +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 diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 76e5e17ba0b..2d6ec171fc3 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -556,15 +556,65 @@ bool VelocityPolygon::validateSteering( const SubPolygonParameter * current_field = findField(current_sw_speed, current_sa); debug_msg.current_field_name = current_field ? current_field->velocity_polygon_name_ : ""; + if (current_field == nullptr) { + RCLCPP_WARN( + logger_, + "[%s] validateSteering: current_field is null — no field matches current velocity. " + "odom_vel=(%.3f, %.3f, %.3f), current_sw_speed=%.3f, current_sa=%.3f, " + "cmd_vel=(%.3f, %.3f, %.3f), target_sw_speed=%.3f, target_sa=%.3f. " + "Check that velocity polygon field ranges cover all reachable velocities.", + polygon_name_.c_str(), + odom_vel.x, odom_vel.y, odom_vel.tw, current_sw_speed, current_sa, + cmd_vel_in.x, cmd_vel_in.y, cmd_vel_in.tw, target_sw_speed, target_steering_angle); + debug_msg.modified = false; + debug_msg.result_vel_x = result_vel.x; + debug_msg.result_vel_y = result_vel.y; + debug_msg.result_vel_tw = result_vel.tw; + steering_debug_pub_->publish(debug_msg); + return false; + } + + // 2. Same-bucket speed limit (always enforced, even when target is in a different bucket, + // because the robot is still physically in the current bucket during any steering transition). + // Check one field up from the current field at the current physical angle: + // - Next field exists and is collision-free → current bucket allows up to next field's max + // - Next field exists and has obstacles → current bucket allows up to current field's max + // - No next field exists → current bucket allows up to current field's max + bool forward_current = current_sw_speed >= 0; + auto fields_at_current_angle = findFieldsForAngle(current_sa, forward_current); + double current_bucket_limit_sw = forward_current ? + current_field->linear_max_ : current_field->linear_min_; + + for (size_t i = 0; i < fields_at_current_angle.size(); i++) { + if (fields_at_current_angle[i] != current_field) { + continue; + } + + if (i + 1 < fields_at_current_angle.size()) { + const SubPolygonParameter * next_field = fields_at_current_angle[i + 1]; + checked_field = next_field; + debug_msg.next_field_name = next_field->velocity_polygon_name_; + int pts = getPointsInsideSubPolygon(*next_field, collision_points_map); + debug_msg.next_field_collision_pts = pts; + if (pts < min_points_) { + // Next field is collision-free — allow up to next field's max + current_bucket_limit_sw = forward_current ? + next_field->linear_max_ : next_field->linear_min_; + } + // else: next field has obstacles — stay at current field's max + } + // else: no faster field — stay at current field's max + break; + } - // 2. Check if target angle is in same bucket (angle range) as current - bool same_bucket = current_field != nullptr && + // 3. Check if target angle is in same bucket (angle range) as current + bool same_bucket = target_steering_angle >= current_field->steering_angle_min_ && target_steering_angle <= current_field->steering_angle_max_; debug_msg.same_bucket = same_bucket; if (same_bucket) { - // Skip same-bucket limiting when the robot is at or near standstill. + // 3a. Skip same-bucket limiting when the robot is at or near standstill. // The current field at standstill (e.g. creeping/stopped) is not meaningful // for speed limiting — the robot must be free to start moving. if (std::abs(current_sw_speed) < low_speed_threshold_) { @@ -576,45 +626,11 @@ bool VelocityPolygon::validateSteering( return false; } - // Check one field up from the current field at the current physical angle. - // Always limit speed to at most the next reachable field's max: - // - Next field exists and is collision-free → limit to next field's max - // - Next field exists and has obstacles → limit to current field's max - // - No next field exists → limit to current field's max - // This ensures progressive speed increase (one field per cycle) and - // prevents exceeding defined field ranges into e-stop territory. - bool forward = current_sw_speed >= 0; - auto fields_at_angle = findFieldsForAngle(current_sa, forward); - const SubPolygonParameter * limit_field = current_field; - - // Find the current field in the sorted list - for (size_t i = 0; i < fields_at_angle.size(); i++) { - if (fields_at_angle[i] != current_field) { - continue; - } - - if (i + 1 < fields_at_angle.size()) { - const SubPolygonParameter * next_field = fields_at_angle[i + 1]; - checked_field = next_field; - debug_msg.next_field_name = next_field->velocity_polygon_name_; - int pts = getPointsInsideSubPolygon(*next_field, collision_points_map); - debug_msg.next_field_collision_pts = pts; - if (pts < min_points_) { - // Next field is collision-free — allow up to next field's max - limit_field = next_field; - } - // else: next field has obstacles — stay at current field's max - } - // else: no faster field — stay at current field's max - break; - } - - double limit_sw = forward ? - limit_field->linear_max_ : limit_field->linear_min_; + // 3b. Limit speed to current bucket's speed limit from step 2 double result_sw_speed = baselinkToSteeringSpeed(result_vel.x, result_vel.tw); - if (std::abs(result_sw_speed) > std::abs(limit_sw)) { - double limited_x = limit_sw * std::cos(target_steering_angle); - double limited_tw = limit_sw * std::sin(target_steering_angle) / wheelbase_; + if (std::abs(result_sw_speed) > std::abs(current_bucket_limit_sw)) { + double limited_x = current_bucket_limit_sw * std::cos(target_steering_angle); + double limited_tw = current_bucket_limit_sw * std::sin(target_steering_angle) / wheelbase_; debug_msg.speed_limit_applied = limited_x; result_vel.x = limited_x; result_vel.tw = limited_tw; @@ -650,24 +666,7 @@ bool VelocityPolygon::validateSteering( return modified; } - // 3. Different bucket — find neighbouring bucket (one step in steering direction) - if (current_field == nullptr) { - RCLCPP_WARN( - logger_, - "[%s] validateSteering: current_field is null — no field matches current velocity. " - "odom_vel=(%.3f, %.3f, %.3f), current_sw_speed=%.3f, current_sa=%.3f, " - "cmd_vel=(%.3f, %.3f, %.3f), target_sw_speed=%.3f, target_sa=%.3f. " - "Check that velocity polygon field ranges cover all reachable velocities.", - polygon_name_.c_str(), - odom_vel.x, odom_vel.y, odom_vel.tw, current_sw_speed, current_sa, - cmd_vel_in.x, cmd_vel_in.y, cmd_vel_in.tw, target_sw_speed, target_steering_angle); - debug_msg.modified = false; - debug_msg.result_vel_x = result_vel.x; - debug_msg.result_vel_y = result_vel.y; - debug_msg.result_vel_tw = result_vel.tw; - steering_debug_pub_->publish(debug_msg); - return false; - } + // 4–5. Different bucket — find neighbouring bucket (one step in steering direction) double neighbour_angle; if (target_steering_angle > current_sa) { @@ -739,16 +738,21 @@ bool VelocityPolygon::validateSteering( } debug_msg.valid_field_name = valid_field->velocity_polygon_name_; - // 4. Adapt speed and steering angle - // 4a. Limit target speed to valid field's speed boundary (converted to baselink) - // Forward: limit to linear_max; Backward: limit to linear_min + // 6. Adapt speed and steering angle + // 6a. Limit target speed to the minimum of: valid field's max (from step 5) + // and current bucket's speed limit (from step 2). + // The current bucket limit is always enforced because the robot is still + // physically in the current bucket during any steering transition. double valid_limit_sw = (target_sw_speed >= 0) ? valid_field->linear_max_ : valid_field->linear_min_; - double valid_max_baselink = steeringToBaselinkSpeed( - valid_limit_sw, neighbour_angle); - if (std::abs(result_vel.x) > std::abs(valid_max_baselink)) { - debug_msg.speed_limit_applied = valid_max_baselink; - result_vel.x = valid_max_baselink; + double effective_limit_sw = + (std::abs(current_bucket_limit_sw) < std::abs(valid_limit_sw)) ? + current_bucket_limit_sw : valid_limit_sw; + double effective_max_baselink = steeringToBaselinkSpeed( + effective_limit_sw, neighbour_angle); + if (std::abs(result_vel.x) > std::abs(effective_max_baselink)) { + debug_msg.speed_limit_applied = effective_max_baselink; + result_vel.x = effective_max_baselink; // Also adjust tw to match the neighbour bucket boundary angle, otherwise // reducing x while keeping tw creates an absurd steering angle // that falls outside all configured field buckets. @@ -758,10 +762,11 @@ bool VelocityPolygon::validateSteering( modified = true; } - // 4b. Only limit steering angle if current speed is larger than max valid speed + // 6b. Only limit steering angle if current speed is larger than max valid speed + double valid_max_baselink = steeringToBaselinkSpeed(valid_limit_sw, neighbour_angle); double current_baselink_abs = std::abs(current_speed); double valid_max_baselink_abs = std::abs(valid_max_baselink); - if (current_baselink_abs > valid_max_baselink_abs && current_field != nullptr) { + if (current_baselink_abs > valid_max_baselink_abs) { double limited_sa; if (target_steering_angle > current_sa) { limited_sa = current_field->steering_angle_max_; diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index 548ce9062f7..878c7099c57 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -1524,6 +1524,178 @@ TEST_F(Tester, testValidateSteeringSameBucket90DegLimitsTw) EXPECT_LE(std::abs(result_sw), 0.3 + 1e-6); } +// ==================== Step 2 current-bucket limit enforced across buckets ==================== + +TEST_F(Tester, testValidateSteeringDifferentBucketCurrentBucketLimitEnforced) +{ + // Setup: straight has slow[0,0.5]+fast[0.5,1.0]; left has slow[0,0.5]+fast[0.5,1.0] + // Robot is in straight_slow at 0.3. Next field (straight_fast) is collision-free → + // current bucket allows up to 1.0. Neighbour (left) valid field allows up to 1.0. + // min(1.0, 1.0) = 1.0 → no speed clamping needed. + // + // Now place collision in straight_fast → current bucket allows only 0.5. + // Neighbour left_fast is collision-free → valid field allows 1.0. + // min(0.5, 1.0) = 0.5 → speed must be clamped to 0.5 even though left allows more. + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, + {"straight_slow", "straight_fast", "left_slow", "left_fast"}); + addSteeringAngleSubPolygon("straight_slow", 0.0, 0.5, -0.1, 0.1, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("straight_fast", 0.5, 1.0, -0.1, 0.1, STEERING_POLYGON_FAST_STR); + addSteeringAngleSubPolygon("left_slow", 0.0, 0.5, 0.1, 0.5, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("left_fast", 0.5, 1.0, 0.1, 0.5, STEERING_POLYGON_FAST_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity vel{0.3, 0.0, 0.0}; + velocity_polygon_->updatePolygon(vel); + + // Current: straight at 0.3 (slow field), Target: left at 0.7 + nav2_collision_monitor::Velocity odom_vel{0.3, 0.0, 0.0}; + double target_sa = 0.3; + double target_tw = std::tan(target_sa) * 0.7 / WHEELBASE; + nav2_collision_monitor::Velocity cmd_vel{0.7, 0.0, target_tw}; + + std::unordered_map> collision_map; + // Collision points inside straight_fast polygon (the larger polygon) + collision_map["source"] = { + {1.2, 0.0}, + {1.3, 0.1}, + }; + + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->validateSteering(cmd_vel, odom_vel, collision_map, action); + EXPECT_TRUE(modified); + EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); + // Current bucket limit is straight_slow max = 0.5 (because straight_fast has collision). + // Neighbour valid field (left_fast) allows 1.0. min(0.5, 1.0) = 0.5. + // At neighbour_angle = 0.1, baselink = 0.5 * cos(0.1) + double neighbour_angle = 0.1; + double expected_max = 0.5 * std::cos(neighbour_angle); + EXPECT_LE(std::abs(action.req_vel.x), expected_max + 1e-3); +} + +TEST_F(Tester, testValidateSteeringDifferentBucketCurrentBucketNoCollisionAllowsMore) +{ + // Same setup but no collision in straight_fast → current bucket allows up to 1.0. + // Neighbour left has only slow[0,0.5] → valid field allows 0.5. + // min(1.0, 0.5) = 0.5 → speed clamped by neighbour, not current bucket. + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, + {"straight_slow", "straight_fast", "left_slow"}); + addSteeringAngleSubPolygon("straight_slow", 0.0, 0.5, -0.1, 0.1, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("straight_fast", 0.5, 1.0, -0.1, 0.1, STEERING_POLYGON_FAST_STR); + addSteeringAngleSubPolygon("left_slow", 0.0, 0.5, 0.1, 0.5, STEERING_POLYGON_SLOW_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity vel{0.3, 0.0, 0.0}; + velocity_polygon_->updatePolygon(vel); + + // Current: straight at 0.3 (slow field), Target: left at 0.7 + nav2_collision_monitor::Velocity odom_vel{0.3, 0.0, 0.0}; + double target_sa = 0.3; + double target_tw = std::tan(target_sa) * 0.7 / WHEELBASE; + nav2_collision_monitor::Velocity cmd_vel{0.7, 0.0, target_tw}; + + std::unordered_map> collision_map; + collision_map["source"] = {}; // no collision + + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->validateSteering(cmd_vel, odom_vel, collision_map, action); + EXPECT_TRUE(modified); + EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); + // Current bucket limit = 1.0 (next field collision-free). Neighbour valid = 0.5. + // min(1.0, 0.5) = 0.5 at neighbour_angle = 0.1 + double neighbour_angle = 0.1; + double expected_max = 0.5 * std::cos(neighbour_angle); + EXPECT_LE(std::abs(action.req_vel.x), expected_max + 1e-3); +} + +TEST_F(Tester, testValidateSteeringDifferentBucketBothLimitsApply) +{ + // Current bucket has collision in next field → bucket limit = 0.5 + // Neighbour bucket valid field also has max 0.5 (only slow field, collision-free) + // min(0.5, 0.5) = 0.5 → both limits agree, speed clamped + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, + {"straight_slow", "straight_fast", "left_slow"}); + addSteeringAngleSubPolygon("straight_slow", 0.0, 0.5, -0.1, 0.1, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon("straight_fast", 0.5, 1.0, -0.1, 0.1, STEERING_POLYGON_FAST_STR); + addSteeringAngleSubPolygon("left_slow", 0.0, 0.5, 0.1, 0.5, STEERING_POLYGON_SLOW_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity vel{0.3, 0.0, 0.0}; + velocity_polygon_->updatePolygon(vel); + + nav2_collision_monitor::Velocity odom_vel{0.3, 0.0, 0.0}; + double target_sa = 0.3; + double target_tw = std::tan(target_sa) * 0.7 / WHEELBASE; + nav2_collision_monitor::Velocity cmd_vel{0.7, 0.0, target_tw}; + + std::unordered_map> collision_map; + // Collision in straight_fast + collision_map["source"] = { + {1.2, 0.0}, + {1.3, 0.1}, + }; + + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->validateSteering(cmd_vel, odom_vel, collision_map, action); + EXPECT_TRUE(modified); + EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); + double neighbour_angle = 0.1; + double expected_max = 0.5 * std::cos(neighbour_angle); + EXPECT_LE(std::abs(action.req_vel.x), expected_max + 1e-3); +} + +TEST_F(Tester, testValidateSteeringBackwardDifferentBucketCurrentBucketLimitEnforced) +{ + // Backward driving: straight has backward_slow[-0.5,0]+backward_fast[-1.0,-0.5] + // Left has backward_slow[-0.5,0] only. + // Robot at backward_slow. Collision in backward_fast → current bucket limit = -0.5. + // Neighbour left backward_slow allows -0.5. min(0.5, 0.5) = 0.5. + setSteeringVelocityPolygonParameters(WHEELBASE, LOW_SPEED_THRESHOLD, + {"straight_bw_slow", "straight_bw_fast", "left_bw_slow"}); + addSteeringAngleSubPolygon( + "straight_bw_slow", -0.5, 0.0, -0.1, 0.1, STEERING_POLYGON_SLOW_STR); + addSteeringAngleSubPolygon( + "straight_bw_fast", -1.0, -0.5, -0.1, 0.1, STEERING_POLYGON_FAST_STR); + addSteeringAngleSubPolygon( + "left_bw_slow", -0.5, 0.0, 0.1, 0.5, STEERING_POLYGON_SLOW_STR); + createSteeringVelocityPolygon("limit"); + + nav2_collision_monitor::Velocity vel{-0.3, 0.0, 0.0}; + velocity_polygon_->updatePolygon(vel); + + // Current: straight backward at -0.3, Target: left backward at -0.7 + nav2_collision_monitor::Velocity odom_vel{-0.3, 0.0, 0.0}; + double target_sa = 0.3; + double target_tw = std::tan(target_sa) * std::abs(-0.7) / WHEELBASE; + // For backward driving, tw sign is inverted + target_tw = -target_tw; + nav2_collision_monitor::Velocity cmd_vel{-0.7, 0.0, target_tw}; + + std::unordered_map> collision_map; + // Collision in backward_fast polygon + collision_map["source"] = { + {-0.6, 0.0}, + {-0.7, 0.1}, + }; + + nav2_collision_monitor::Action action{ + nav2_collision_monitor::DO_NOTHING, cmd_vel, ""}; + + bool modified = velocity_polygon_->validateSteering(cmd_vel, odom_vel, collision_map, action); + EXPECT_TRUE(modified); + EXPECT_EQ(action.action_type, nav2_collision_monitor::LIMIT); + // Speed should be limited: current bucket limit is -0.5 (backward_fast has collision). + // At neighbour angle boundary, baselink = -0.5 * cos(0.1) + double neighbour_angle = 0.1; + double expected_min = -0.5 * std::cos(neighbour_angle); + EXPECT_GE(action.req_vel.x, expected_min - 1e-3); // not more negative +} + int main(int argc, char ** argv) { // Initialize the system