diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index b8722a96f7c..cb36b0b2925 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -103,3 +103,89 @@ The zones around the robot and the data sources are the same as for the Collisio Detailed configuration parameters, their description and how to setup a Collision Detector could be found at its [Configuration Guide](https://docs.nav2.org/configuration/packages/collision_monitor/configuring-collision-detector-node.html). The `CollisionMonitor` node makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). + + +### Lidar estop prevention + +We use this with a safety lidar with e-stop zones depending on speed and steering angle. We want to avoid hitting such a zone. +Thus, the collision monitor must ensure to limit the speed and steering angle so that the field chosen by +the lidar is not intersecting with any lidar points (that would trigger an estop). + +#### Terminology + +* **Bucket**: a steering angle range (e.g. -12° to 12°). A bucket groups multiple fields that share the same angle range. +* **Field**: a specific combination of steering angle bucket and velocity range. Each field corresponds to one `SubPolygonParameter` / velocity polygon entry. + +During field set creation, we also generate corresponding velocity polygons. They are about 50% larger than the corresponding +e-stop zone. Example. Note that linear speed is for the steering wheel, not the baselink. + +```yaml +forward_straight_mid_3: + points: # polygon here + linear_min: 0.5 + linear_max: 0.7 + steering_angle_min: -0.20944 # -12 degrees in radians + steering_angle_max: 0.20944 # 12 degrees in radians + linear_limit: 0.49 # steering wheel speed, not base link speed +``` + +Here, the **bucket** is the angle range [-12°, 12°]. The **field** is that bucket combined with the speed range [0.5, 0.7]. + +So - if we are between 0.5 and 0.7 m/s, and hit that warning field, we must reduce our speed to *below* that field - because there is an obstacle in the polygon, +which, if we approach it further, would trigger an estop. +Once we are at 0.49, we will be in another field with a smaller polygon on the lidar side. Thus, the velocity polygon is also smaller. If we keep approaching the obstacle, we would slow down further. +If the obstacle e.g. moves with us or is on the side, we can keep that speed. + +### Step 1: Normal collision monitor with velocity polygon + +### Step 2: Steering validation + +All speed comparisons against the low threshold use **steering wheel speed** (which accounts for the steering angle), except for the direction reversal check which uses baselink speed. + +We always assume that a matching field exists for the current velocity. If no field matches (e.g. the robot is outside all configured speed ranges), a warning is logged and steering validation is skipped for that cycle. + +If speed goes through zero (so sign(target speed) <> sign(current speed)): + +* if abs(current baselink speed) > low threshold → keep steering angle (we must anyway just slow down asap) +* if abs(current baselink speed) < low threshold → allow steering + +else: + +1. check if both abs(target steering wheel speed) and abs(current steering wheel speed) are < low threshold. If yes → done +2. check the one-step faster field in the **current** bucket for collision (same-bucket speed limit). Only the one-step faster field needs to be checked, even if target speed is in a much faster field. If the next field is collision-free → current bucket allows up to next field's max. If the next field has obstacles or no faster field exists → current bucket allows up to current field's max. This limit is always enforced — including when the target angle is in a different bucket — because the robot is still physically in the current bucket during any steering transition. +3. check if target angle is in same bucket as current angle. If yes → + 1. if abs(current steering wheel speed) < low threshold → done (robot is at standstill and must be free to start moving, the current field is not meaningful for speed limiting) + 2. limit speed to the current bucket's speed limit from step 2. → done +4. if target angle is in a different bucket → determine the direction of steering and find the neighbouring bucket from current angle in that direction. Only one bucket step at a time, even if the target angle is several buckets away. +5. in the neighbouring bucket, determine max speed / valid field + 1. start at fastest possible field (field for max(current speed, target speed)). If that is in collision, go down until a collision-free one is found. That one we call “valid” field. If all fields are in collision, use the slowest one with same speed sign in target direction (that is allowed even if in collision) +6. now adapt speed and steering angle + 1. limit target speed to the **minimum** of: max speed of valid field (from 5) and current bucket's speed limit (from 2) + 2. if current speed is larger than max valid speed: limit steering angle to boundary of current bucket + +→ done + +After some iterations, the current speed will be in the valid field → AMR will be allowed to steer, as in 6b, the current speed will not be above max valid speed + +### Field exceedance prevention + +A fundamental safety invariant: **the commanded speed must never exceed the fastest configured field for the current steering angle bucket.** Violating this would cause the safety lidar to activate an e-stop, since no matching protective field exists for that speed/angle combination. + +#### Rule 1: Speed must stay within the available fields of the current bucket + +For any given steering angle, the lidar has a finite set of configured fields (each covering a speed range). The collision monitor must clamp the commanded speed so that it never exceeds the maximum speed of the fastest field defined for that bucket. If a bucket only has fields up to 0.5 m/s, the robot must not be commanded above 0.5 m/s while in that bucket — regardless of what the planner requests. + +This applies at all times, not only when obstacles are detected. Even in free space, sending a speed that has no corresponding field for the current angle would be unsafe. + +If no fields exist at all for the current physical steering angle, the velocity is set to zero. + +#### Rule 2: High speed only after entering the corresponding bucket + +When the robot transitions from a fully turned position (where only slow-speed fields exist) toward a straight position (where high-speed fields are available), the higher speed must **not** be sent until the steering angle has actually entered the bucket that supports that speed. + +Example: if the robot is fully turned left at 0.3 m/s and the planner requests straight driving at 1.0 m/s, the sequence must be: +1. The robot begins straightening the steering while remaining at the slow speed allowed by the current (turned) bucket. +2. Only once the steering angle crosses into the straight bucket — which has fields defined up to 1.0 m/s — may the speed be increased. +3. Speed increases follow the normal one-field-step-at-a-time logic from Step 2. + +The opposite direction (straight → turned) is handled naturally: as the steering angle enters a more turned bucket with lower max speed, Rule 1 immediately clamps the speed down. \ 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..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" @@ -267,8 +268,12 @@ class CollisionMonitor : public nav2::LifecycleNode rclcpp::Time stop_stamp_; /// @brief Timeout after which 0-velocity ceases to be published rclcpp::Duration stop_pub_timeout_; + /// @brief Whether steering validation is enabled + bool enable_steering_validation_; /// @brief Active polygons publisher rclcpp::Publisher::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/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/include/nav2_collision_monitor/velocity_polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp index afc855cdde3..bba1ca06a67 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" @@ -24,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" @@ -74,6 +77,31 @@ class VelocityPolygon : public Polygon */ void updatePolygon(const Velocity & cmd_vel_in) override; + /** + * @brief Validates steering to prevent triggering lidar e-stop zones. + * Implements Step 2 of the lidar e-stop prevention algorithm. + * @param cmd_vel_in Desired robot velocity (target) + * @param odom_vel Current robot velocity from odometry + * @param collision_points_map Map of source name to collision points + * @param robot_action Output robot action to modify if steering is restricted + * @return True if velocity was modified by steering validation + */ + bool validateSteering( + const Velocity & cmd_vel_in, + const Velocity & odom_vel, + const std::unordered_map> & collision_points_map, + Action & robot_action); + + /** + * @brief Clamp commanded speed to the max available field at the current physical angle. + * Final safety gate to prevent field exceedance within the current bucket and + * during turned→straight transitions. + * @param odom_vel Current robot velocity from odometry (physical state) + * @param robot_action Robot action to modify if speed exceeds max field + * @return True if velocity was clamped + */ + bool clampToMaxField(const Velocity & odom_vel, Action & robot_action); + protected: /** * @brief Custom struct to store the parameters of the sub-polygon @@ -119,8 +147,80 @@ class VelocityPolygon : public Polygon */ bool isInRange(const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon_param); + /** + * @brief Compute steering angle from linear and angular velocity + * @param vel Velocity containing linear.x and angular.z + * @return Steering angle in radians + */ + double computeSteeringAngle(const Velocity & vel) const; + + /** + * @brief Convert baselink speed to steering wheel speed + * @param linear_vel Baselink linear velocity (twist.linear.x) + * @param angular_vel Baselink angular velocity (twist.angular.z) + * @return Steering wheel speed + */ + double baselinkToSteeringSpeed(double linear_vel, double angular_vel) const; + + /** + * @brief Convert steering wheel speed to baselink speed + * @param steering_speed Steering wheel speed + * @param steering_angle Steering angle in radians + * @return Baselink speed + */ + double steeringToBaselinkSpeed(double steering_speed, double steering_angle) const; + + /** + * @brief Convert steering angle back to angular velocity (twist.angular.z) + * @param baselink_speed Baselink linear speed + * @param steering_angle Steering angle in radians + * @return Angular velocity + */ + double steeringAngleToTw(double baselink_speed, double steering_angle) const; + + /** + * @brief Find the field (sub-polygon) matching a given steering wheel speed and steering angle + * @param steering_wheel_speed Speed in steering wheel frame + * @param steering_angle Steering angle in radians + * @return Pointer to matching sub-polygon, or nullptr if none found + */ + const SubPolygonParameter * findField( + double steering_wheel_speed, double steering_angle) const; + + /** + * @brief Find all fields (sub-polygons) matching a given steering angle and direction + * @param steering_angle Steering angle in radians + * @param forward If true, return only forward fields (linear_max >= 0); + * if false, return only backward fields (linear_min <= 0) + * @return Vector of pointers to matching sub-polygons, sorted slowest (closest to zero) first + */ + std::vector 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_; + // 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 @@ -130,6 +230,8 @@ class VelocityPolygon : public Polygon double current_steering_angle_; /// @brief Distance between front and rear axes double wheelbase_; + /// @brief Speed below which steering is freely allowed + double low_speed_threshold_; /// @brief Vector to store the parameters of the sub-polygon std::vector 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..bb386ffd5b7 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( @@ -289,6 +292,8 @@ bool CollisionMonitor::getParameters( stop_pub_timeout_ = rclcpp::Duration::from_seconds( node->declare_or_get_parameter("stop_pub_timeout", 1.0)); + enable_steering_validation_ = node->declare_or_get_parameter("enable_steering_validation", true); + if ( !configureSources( base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction)) @@ -427,6 +432,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: { // Current timestamp for all inner routines prolongation rclcpp::Time curr_time = this->now(); + const auto process_start = std::chrono::steady_clock::now(); // Do nothing if main worker in non-active state if (!process_active_) { @@ -451,6 +457,10 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: if (!source->getData(curr_time, iter.first->second) && source->getSourceTimeout().seconds() != 0.0) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 2000, + "[%s]: Source getData() failed (no data / stale / TF error) — triggering STOP", + source->getSourceName().c_str()); action_polygon = nullptr; robot_action.polygon_name = "invalid source"; robot_action.action_type = STOP; @@ -459,6 +469,12 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: robot_action.req_vel.tw = 0.0; break; } + if (iter.first->second.empty()) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 2000, + "[%s]: Source enabled and getData() succeeded but returned 0 points", + source->getSourceName().c_str()); + } } if (collision_points_marker_pub_->get_subscription_count() > 0) { @@ -499,6 +515,8 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: std::abs(last_odom_msg_.linear.y) < velocity_threshold && std::abs(last_odom_msg_.angular.z) < velocity_threshold; + std::shared_ptr active_limit_vel_polygon; + for (std::shared_ptr polygon : polygons_) { if (!polygon->getEnabled() || !enabled_) { continue; @@ -529,6 +547,14 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: polygon->updatePolygon({last_odom_msg_.linear.x, last_odom_msg_.linear.y, last_odom_msg_.angular.z}); } + // Track the active LIMIT VelocityPolygon (the one matching current speed/steering angle) + if (polygon->getActionType() == LIMIT) { + auto vp = std::dynamic_pointer_cast(polygon); + if (vp && vp->getCurrentSubPolygonName() != "none") { + active_limit_vel_polygon = vp; + } + } + const ActionType at = polygon->getActionType(); if (at == STOP || at == SLOWDOWN || at == LIMIT) { // Process STOP/SLOWDOWN for the selected polygon @@ -545,6 +571,32 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: } } + // Step 2: Steering validation (only for the active LIMIT VelocityPolygon) + if (enable_steering_validation_ && active_limit_vel_polygon && + robot_action.action_type != STOP) + { + Velocity odom_vel{last_odom_msg_.linear.x, last_odom_msg_.linear.y, last_odom_msg_.angular.z}; + if (active_limit_vel_polygon->validateSteering( + cmd_vel_in, odom_vel, sources_collision_points_map, robot_action)) + { + action_polygon = active_limit_vel_polygon; + } + } + + // Step 3: Field exceedance prevention — clamp speed to max available field + if (enable_steering_validation_) { + for (auto polygon : polygons_) { + auto vel_polygon = std::dynamic_pointer_cast(polygon); + if (!vel_polygon || !polygon->getEnabled()) { + continue; + } + Velocity odom_vel{last_odom_msg_.linear.x, last_odom_msg_.linear.y, last_odom_msg_.angular.z}; + if (vel_polygon->clampToMaxField(odom_vel, robot_action)) { + action_polygon = polygon; + } + } + } + if ((robot_action.polygon_name != robot_action_prev_.polygon_name) && enabled_) { // Report changed robot behavior notifyActionState(robot_action, action_polygon); @@ -570,6 +622,12 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: active_polygons_pub_->publish(std::move(msg)); robot_action_prev_ = robot_action; + + // Publish processing time + std_msgs::msg::Float32 time_msg; + time_msg.data = std::chrono::duration( + std::chrono::steady_clock::now() - process_start).count(); + processing_time_pub_->publish(time_msg); } bool CollisionMonitor::processStopSlowdownLimit( 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..6ca08566f12 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)) { @@ -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( diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index d082fcfc53f..2d6ec171fc3 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -14,7 +14,12 @@ #include "nav2_collision_monitor/velocity_polygon.hpp" +#include +#include +#include + #include "nav2_ros_common/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" namespace nav2_collision_monitor { @@ -59,6 +64,18 @@ bool VelocityPolygon::getParameters( polygon_name_ + ".holonomic", false); 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); for (std::string velocity_polygon_name : velocity_polygons) { // polygon points parameter @@ -199,6 +216,13 @@ bool VelocityPolygon::getParameters( ex.what()); return false; } + + 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; } @@ -222,7 +246,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 +273,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 +347,528 @@ 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) +{ + return nav2_util::geometry_utils::isPointInsidePolygon(point.x, point.y, vertices); +} + +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; + } + + // 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_; + 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; + + 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); + + 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; + + // 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; + debug_msg.steering_angle_limit = current_sa; + } + // 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; + } + 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 + // 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; + 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; + } + + // For field lookup, negate the steering angle when driving backward: + // the swept area flips, so backward + far-left needs the far-right polygon. + 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; + } + + // 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) { + // 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_) { + 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; + } + + // 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(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; + modified = true; + } + + // Same bucket → done + 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); + + // 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; + } + + // 4–5. Different bucket — find neighbouring bucket (one step in steering direction) + + 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()) { + 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; + } + + // 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 speed is beyond all fields, start from the fastest + if (i == 0) { + start_idx = static_cast(neighbour_fields.size()) - 1; + } + } + + // 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_) { + // 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 + } + checked_field = valid_field; + } + debug_msg.valid_field_name = valid_field->velocity_polygon_name_; + + // 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 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. + // 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; + } + + // 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) { + double limited_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; + result_vel.tw = steeringAngleToTw(result_vel.x, limited_sa); + 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); + + // 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; +} + +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/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..878c7099c57 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,65 @@ 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); + } + + 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 @@ -153,8 +213,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 +403,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 +690,1012 @@ 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); + // Speed (0.3) is already within the slowest field's max (0.5), so nothing to change + EXPECT_FALSE(modified); +} + +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 +} + +// ==================== 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); +} + + +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); +} + +// ==================== 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 diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 0ec9e393d57..605026d8869 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -47,6 +47,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..b630b5a8b50 --- /dev/null +++ b/nav2_msgs/msg/SteeringValidationDebug.msg @@ -0,0 +1,57 @@ +# Debug output from VelocityPolygon::validateSteering +std_msgs/Header header + +# Input velocities +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 +float32 cmd_vel_x +float32 cmd_vel_y +float32 cmd_vel_tw + +# odom_vel components +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 +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) +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 +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 +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 +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