diff --git a/nav2_bringup/launch/tb4_simulation_launch.py b/nav2_bringup/launch/tb4_simulation_launch.py index ba5266ee64f..dc74cac9dd4 100644 --- a/nav2_bringup/launch/tb4_simulation_launch.py +++ b/nav2_bringup/launch/tb4_simulation_launch.py @@ -188,7 +188,7 @@ def generate_launch_description() -> LaunchDescription: ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', default_value='True', description='Whether to execute gzclient)' + 'headless', default_value='False', description='Whether to execute gzclient)' ) declare_world_cmd = DeclareLaunchArgument( diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 053ab6693c9..cf877a890c2 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -123,7 +123,7 @@ controller_server: path_length_tolerance: 1.0 PathHandler: plugin: "nav2_controller::FeasiblePathHandler" - prune_distance: 2.0 + prune_distance: 10.0 #TODO enforce_path_inversion: false enforce_path_rotation: false inversion_xy_tolerance: 0.2 @@ -172,6 +172,7 @@ controller_server: "GoalCritic", "GoalAngleCritic", "PathAlignCritic", + "ObstacleBypassCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic", @@ -210,15 +211,25 @@ controller_server: cost_power: 1 cost_weight: 14.0 max_path_occupancy_ratio: 0.05 + min_distance_occupancy_check: 4.5 # TODO his: 4.5. my horizon is 2.8, so try 3.3? trajectory_point_step: 4 threshold_to_consider: 0.5 offset_from_furthest: 20 use_path_orientations: false + ObstacleBypassCritic: + enabled: true + cost_power: 1 + cost_weight: 4.667 #TODO + bypass_offset_dist: 1.0 #TODO + max_path_occupancy_ratio: 0.05 + min_distance_occupancy_check: 4.5 #TODO + threshold_to_consider: 0.5 #TODO + offset_from_furthest: 20 #TODO PathFollowCritic: enabled: true cost_power: 1 cost_weight: 5.0 - offset_from_furthest: 5 + offset_from_furthest: 5 #TODO threshold_to_consider: 1.4 PathAngleCritic: enabled: true @@ -241,9 +252,13 @@ local_costmap: global_frame: odom robot_base_frame: base_link rolling_window: true - width: 3 - height: 3 + width: 10 + height: 10 resolution: 0.05 + # footprint: "[ [2.230, 0.65], [-1.4312, 0.65], [-1.4312, -0.65], [2.230, -0.65] ]" + # footprint_padding: 0.1 + # footprint: "[ [0.725, 0.525], [0.725, -0.525], [-0.725, -0.525], [-0.725, 0.525] ]" + # footprint_padding: 0.12 robot_radius: 0.22 plugins: ["voxel_layer", "inflation_layer"] filters: ["keepout_filter"] @@ -255,8 +270,8 @@ local_costmap: lethal_override_cost: 200 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.70 + cost_scaling_factor: 0.7 #TODO + inflation_radius: 5.0 #TODO voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: true @@ -279,9 +294,9 @@ local_costmap: clearing: true marking: true data_type: "LaserScan" - raytrace_max_range: 3.0 + raytrace_max_range: 10.0 # TODO 10.0 raytrace_min_range: 0.0 - obstacle_max_range: 2.5 + obstacle_max_range: 9.5 # TODO 9.5 obstacle_min_range: 0.0 static_layer: plugin: "nav2_costmap_2d::StaticLayer" @@ -299,7 +314,7 @@ global_costmap: robot_radius: 0.22 resolution: 0.05 track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + plugins: ["static_layer", "inflation_layer"] filters: ["keepout_filter", "speed_filter"] keepout_filter: plugin: "nav2_costmap_2d::KeepoutFilter" diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 1a810403f6b..1fe68fa7b4a 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -94,6 +94,7 @@ add_library(mppi_critics SHARED src/critics/cost_critic.cpp src/critics/goal_critic.cpp src/critics/goal_angle_critic.cpp + src/critics/obstacle_bypass_critic.cpp src/critics/obstacles_critic.cpp src/critics/path_align_critic.cpp src/critics/path_angle_critic.cpp diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index e2635180429..93e3d36e6fc 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -146,7 +146,8 @@ Uses inflated costmap cost directly to avoid obstacles | threshold_to_consider | double | Default 0.5. Distance between robot and goal above which path align cost stops being considered | | offset_from_furthest | double | Default 20. Checks that the candidate trajectories are sufficiently far along their way tracking the path to apply the alignment critic. This ensures that path alignment is only considered when actually tracking the path, preventing awkward initialization motions preventing the robot from leaving the path to achieve the appropriate heading. | | trajectory_point_step | int | Default 4. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable. | - | max_path_occupancy_ratio | double | Default 0.07 (7%). Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene. | + | max_path_occupancy_ratio | double | Default 0.07 (7%). Maximum proportion of a path segment that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene. | + | min_distance_occupancy_check | double | Default 2.0. Min distance for the occupancy check's path segment. The segment length is ``min(min_distance_occupancy_check, distance_to_furthest_reached_path_point)`` | | use_path_orientations | bool | Default false. Whether to consider path's orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false. | #### Path Angle Critic @@ -190,6 +191,20 @@ Uses inflated costmap cost directly to avoid obstacles | cost_power | int | Default 1. Power order to apply to term. | | deadband_velocities | double[] | Default [0.0, 0.0, 0.0]. The array of deadband velocities [vx, vz, wz]. A zero array indicates that the critic will take no action. | +#### Obstacle Bypass Critic + +Steers the robot around dynamic obstacles blocking the path by determining the best side to bypass using the costmap and scoring trajectories toward an offset target point. + + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | cost_weight | double | Default 4.667. Weight to apply to critic term. | + | cost_power | int | Default 1. Power order to apply to term. | + | min_distance_occupancy_check | double | Default 2.0. Minimum distance (m) along the path to check for occupancy to determine if the path is blocked. | + | max_path_occupancy_ratio | double | Default 0.07 (7%). Maximum proportion of the checked path segment that can be occupied before the bypass behavior activates. | + | offset_from_furthest | int | Default 20. Number of path points after the furthest reached point to place the bypass target, controlling how far ahead the robot aims while bypassing. | + | threshold_to_consider | double | Default 0.5. Distance between robot and goal above which the bypass critic stops being considered. | + | bypass_offset_dist | double | Default 1.0. Additional offset distance (m) beyond the first free cell when computing the bypass target point perpendicular to the path. | + ### XML configuration example ``` @@ -316,7 +331,7 @@ Tune std's carefully for low acceleration. If you're seeing a lot of chatter in If you don't require path following behavior (e.g. just want to follow a goal pose and let the model predictive elements decide the best way to accomplish that), you may easily remove the PathAlign, PathFollow and PathAngle critics. -By default, the controller is tuned and has the capabilities established in the PathAlign/Obstacle critics to generally follow the path closely when no obstacles prevent it, but able to deviate from the path when blocked. See `PathAlignCritic::score()` for details, but it is disabled when the local path is blocked so the obstacle critic takes over in that state. +By default, the controller is tuned and has the capabilities established in the PathAlign/Obstacle critics to generally follow the path closely when no obstacles prevent it, but able to deviate from the path when blocked. See `PathAlignCritic::score()` for details, but it is disabled when the local path is blocked so the obstacle critic takes over in that state. The ``ObstacleBypassCritic`` can be helpful to bypass around obstacles on the path, if desired. Just make sure to have a reasoanble prediction horizon (model_dt * time_steps), prune distance, and costmap sensor range to see far enough ahead to start going around an obstacle before approaching it to slow down. As such, `min_distance_occupancy_check >= prediction horizon`, in general. If you want to slow further on approach to goal, consider increasing the ``threshold_to_consider`` parameters to give a hand off from the path tracking critics to the goal approach critics sooner - then tune those critics for your profile of interest. @@ -328,6 +343,10 @@ The same applies to the Path Follow and Align offsets from furthest. In the same The Path Follow critic cannot drive velocities greater than the projectable distance of that velocity on the available path on the rolling costmap. The Path Align critic `offset_from_furthest` represents the number of path points a trajectory passes through while tracking the path. If this is set either absurdly low (e.g. 5) it can trigger when a robot is simply trying to start path tracking causing some suboptimal behaviors and local minima while starting a task. If it is set absurdly high (e.g. 50) relative to the path resolution and costmap size, then the critic may never trigger or only do so when at full-speed. A balance here is wise. A selection of this value to be ~30% of the maximum velocity distance projected is good (e.g. if a planner produces points every 2.5cm, 60 can fit on the 1.5m local costmap radius. If the max speed is 0.5m/s with a 3s prediction time, then 20 points represents 33% of the maximum speed projected over the prediction horizon onto the path). When in doubt, `prediction_horizon_s * max_speed / path_resolution / 3.0` is a good baseline. +If your robot's kinematics is very different than the defaults, you may need to tune ``PathAlignCritic.min_distance_occupancy_check``. This distance defines the min length of the path segment used to disable the ``PathAlignCritic``. A reasonable initial value should be the horizon's length (``= time_step*model_dt``) at full speed. If the distance is too short, the robot may slow down when approaching an obstacle ahead and may even get too close to it and get stuck. If the distance is too big, the critic will be disabled too early, which may cause deviation from path (if for example, the reference point for PathFollowCritic is beyond a curve on the path). + +Note: that a sufficiently long horizon length is needed for this behavior to work well. If the horizon is too short, the controller may not find trajectories around the obstacle. + ### Obstacle, Inflation Layer, and Path Following There also exists a relationship between the costmap configurations and the Obstacle critic configurations. If the Obstacle critic is not well tuned with the costmap parameters (inflation radius, scale) it can cause the robot to wobble significantly as it attempts to take finitely lower-cost trajectories with a slightly lower cost in exchange for jerky motion. The default behavior was tuned for small AMRs (e.g. turtlebots or similar), so if using a larger robot, you may want to reduce the `repulsion_weight` in kind. It may also perform awkward maneuvers when in free-space to try to maximize time in a small pocket of 0-cost over a more natural motion which involves moving into some low-costed region. Finally, it may generally refuse to go into costed space at all when starting in a free 0-cost space if the gain is set disproportionately higher than the Path Follow scoring to encourage the robot to move along the path. This is due to the critic cost of staying in free space becoming more attractive than entering even lightly costed space in exchange for progression along the task. diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index deae2c11cb9..f2dd1a424f9 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -1,6 +1,10 @@ + + mppi critic for steering around dynamic obstacles blocking the path + + mppi critic for obstacle avoidance diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacle_bypass_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacle_bypass_critic.hpp new file mode 100644 index 00000000000..066d389f2b2 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacle_bypass_critic.hpp @@ -0,0 +1,69 @@ +// Copyright (c) 2026 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLE_BYPASS_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLE_BYPASS_CRITIC_HPP_ + +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::ObstacleBypassCritic + * @brief Critic objective function for steering around dynamic obstacles + * blocking the path. Uses the costmap to determine which side of the + * obstacle is best to bypass and how far to offset from the path. + */ +class ObstacleBypassCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic parameters and runtime state. + */ + void initialize() override; + + /** + * @brief Score trajectories based on obstacle bypass objective. + * @param data Critic data containing trajectories, path, and context. + */ + void score(CriticData & data) override; + +protected: + /** + * @brief Determine the best side and distance to bypass an obstacle using costmap + * @param path_x X position on path at obstacle + * @param path_y Y position on path at obstacle + * @param path_yaw Yaw of path tangent at obstacle + * @param[out] signed_offset Signed offset distance (+ left, - right) + * @return true if a valid bypass target was found + */ + bool determineBestBypassSide( + float path_x, float path_y, float path_yaw, float & signed_offset); + + size_t offset_from_furthest_{0}; + float threshold_to_consider_{0}; + float min_distance_occupancy_check_{0}; + float max_path_occupancy_ratio_{0}; + float bypass_offset_dist_{0}; + unsigned int power_{0}; + float weight_{0}; + bool bypass_active_{false}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLE_BYPASS_CRITIC_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index f7ad2fda6a9..f5af270af86 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -48,6 +48,7 @@ class PathAlignCritic : public CriticFunction size_t offset_from_furthest_{0}; int trajectory_point_step_{0}; float threshold_to_consider_{0}; + float min_distance_occupancy_check_{0}; float max_path_occupancy_ratio_{0}; bool use_path_orientations_{false}; unsigned int power_{0}; diff --git a/nav2_mppi_controller/src/critics/obstacle_bypass_critic.cpp b/nav2_mppi_controller/src/critics/obstacle_bypass_critic.cpp new file mode 100644 index 00000000000..f8460d7f3e7 --- /dev/null +++ b/nav2_mppi_controller/src/critics/obstacle_bypass_critic.cpp @@ -0,0 +1,237 @@ +// Copyright (c) 2026 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "nav2_mppi_controller/critics/obstacle_bypass_critic.hpp" +#include "nav2_costmap_2d/cost_values.hpp" + +namespace mppi::critics +{ + +void ObstacleBypassCritic::initialize() +{ + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 4.667f); + getParam(min_distance_occupancy_check_, "min_distance_occupancy_check", 2.0f); + getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07f); + getParam(offset_from_furthest_, "offset_from_furthest", 20); + getParam(threshold_to_consider_, "threshold_to_consider", 0.5f); + getParam(bypass_offset_dist_, "bypass_offset_dist", 1.0f); + + RCLCPP_INFO( + logger_, + "ObstacleBypassCritic instantiated with %d power and %f weight", + power_, weight_); +} + +bool ObstacleBypassCritic::determineBestBypassSide( + float path_x, float path_y, float path_yaw, float & signed_offset) +{ + const float perp_x = -sinf(path_yaw); + const float perp_y = cosf(path_yaw); + const float resolution = static_cast(costmap_->getResolution()); + const bool tracking_unknown = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + + const int max_steps = static_cast( + std::max(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY())); + unsigned int mx, my; + + auto isNonLethal = [&](unsigned char c) { + return c < nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && + (c != nav2_costmap_2d::NO_INFORMATION || tracking_unknown); + }; + + // Scan perpendicular to the path to find the first non-lethal cell on each side. + auto scanSide = [&](float sign) -> int { + for (int s = 1; s <= max_steps; ++s) { + float wx = path_x + sign * s * resolution * perp_x; + float wy = path_y + sign * s * resolution * perp_y; + if (!costmap_->worldToMap(wx, wy, mx, my)) { + return max_steps + 1; + } else if (isNonLethal(costmap_->getCost(mx, my))) { + return s; + } + } + return max_steps + 1; + }; + + const int first_free_left = scanSide(1.0f); + const int first_free_right = scanSide(-1.0f); + if (first_free_left > max_steps && first_free_right > max_steps) { + return false; + } + + // Prefer the side where free space is closer; break ties to left + // Signed: + left, - right. Distance = first free cell + margin. + float sign = (first_free_left <= first_free_right) ? 1.0f : -1.0f; + int first_free = std::min(first_free_left, first_free_right); + signed_offset = sign * (first_free * resolution + bypass_offset_dist_); + + // Validate the target point on the chosen side + float target_x = path_x + signed_offset * perp_x; + float target_y = path_y + signed_offset * perp_y; + if (costmap_->worldToMap(target_x, target_y, mx, my) && + isNonLethal(costmap_->getCost(mx, my))) + { + return true; + } + + // Try the other side + sign = -sign; + first_free = (sign > 0.0f) ? first_free_left : first_free_right; + if (first_free > max_steps) { + return false; + } + signed_offset = sign * (first_free * resolution + bypass_offset_dist_); + target_x = path_x + signed_offset * perp_x; + target_y = path_y + signed_offset * perp_y; + if (costmap_->worldToMap(target_x, target_y, mx, my) && + isNonLethal(costmap_->getCost(mx, my))) + { + return true; + } + + return false; +} + +void ObstacleBypassCritic::score(CriticData & data) +{ + if (!enabled_ || data.state.local_path_length < threshold_to_consider_) { + return; + } + + // Don't apply when first getting bearing w.r.t. the path + utils::setPathFurthestPointIfNotSet(data); + const size_t furthest_reached_path_point = *data.furthest_reached_path_point; + if (furthest_reached_path_point < offset_from_furthest_) { + return; + } + + // Find the first path IDX further than max(min_distance_occ_check, furthest_reached_path_point) + const size_t path_segments_count = data.path.x.size() - 1; + size_t occupancy_check_distance_idx = 0; + float dx = 0.0f, dy = 0.0f, path_dist = 0.0f; + for (unsigned int i = 1; i != path_segments_count; i++) { + dx = data.path.x(i) - data.path.x(i - 1); + dy = data.path.y(i) - data.path.y(i - 1); + path_dist += sqrtf(dx * dx + dy * dy); + if (path_dist <= min_distance_occupancy_check_ || i < furthest_reached_path_point) { + occupancy_check_distance_idx = (i + 1 < path_segments_count) ? i + 1 : i; + } + } + + // Check if obstacles are blocking significant proportions of the local path + // If path is blocked, incentivize turning in the shorter direction around the obstacle + const float occupancy_check_distance_idx_flt = static_cast(occupancy_check_distance_idx); + utils::setPathCostsIfNotSet(data, costmap_ros_); + std::vector & path_pts_valid = *data.path_pts_valid; + float invalid_ctr = 0.0f; + for (size_t i = 0; i < occupancy_check_distance_idx; i++) { + if (!path_pts_valid[i]) {invalid_ctr += 1.0f;} + } + + const float occupancy_ratio = invalid_ctr / occupancy_check_distance_idx_flt; + const bool path_blocked = occupancy_ratio > max_path_occupancy_ratio_ && invalid_ctr > 2.0f; + + // Once bypass is active, require the ratio to drop well below the + // threshold before deactivating to prevent oscillation + if (!path_blocked) { + if (!bypass_active_ || occupancy_ratio < max_path_occupancy_ratio_ * 0.5f) { + bypass_active_ = false; + return; + } + } + + // Find the first blocked path point + size_t blocked_idx = 0; + for (size_t j = 0; j < occupancy_check_distance_idx; j++) { + if (!path_pts_valid[j]) {blocked_idx = j; break;} + } + + // Find first valid path point past the blocked region + size_t resume_idx = blocked_idx; + for (; resume_idx < path_pts_valid.size(); resume_idx++) { + if (path_pts_valid[resume_idx]) {break;} + } + + // If blocked until the end of the path, don't activate bypass + if (resume_idx >= path_pts_valid.size()) { + bypass_active_ = false; + return; + } + + // Midpoint of blocked region to score against. The path is being continuously + // pruned, so the blocked_idx is updated and adjusted forward as the robot moves + const size_t obstacle_idx = (blocked_idx + resume_idx) / 2; + + // Compute path tangent from XY poses at the obstacle region to determine which + // direction to steer the vehicle to attempt to bypass the obstacle. + const size_t next_idx = std::min(obstacle_idx + 1, path_segments_count - 1); + const float path_x = data.path.x(obstacle_idx); + const float path_y = data.path.y(obstacle_idx); + const float tangent_x = data.path.x(next_idx) - path_x; + const float tangent_y = data.path.y(next_idx) - path_y; + const float tangent_len = sqrtf(tangent_x * tangent_x + tangent_y * tangent_y); + if (tangent_len < 1e-6f) { + bypass_active_ = false; + return; + } + const float path_yaw = atan2f(tangent_y, tangent_x); + + float signed_offset = 0.0f; + if (!determineBestBypassSide(path_x, path_y, path_yaw, signed_offset)) { + bypass_active_ = false; + return; // No valid bypass found + } + + // Score against a forward-looking target point offset from the path + // in the direction of the bypass to incentivize trajectories to steer around + // the obstacle in the direction with the least disruption to path tracking. + const size_t target_idx = std::min( + furthest_reached_path_point + offset_from_furthest_, path_segments_count - 1); + const size_t target_next = std::min(target_idx + 1, path_segments_count - 1); + const float target_tx = data.path.x(target_next) - data.path.x(target_idx); + const float target_ty = data.path.y(target_next) - data.path.y(target_idx); + const float target_tlen = sqrtf(target_tx * target_tx + target_ty * target_ty); + if (target_tlen < 1e-6f) { + bypass_active_ = false; + return; + } + const float perp_x = -target_ty / target_tlen; + const float perp_y = target_tx / target_tlen; + const float target_x = data.path.x(target_idx) + signed_offset * perp_x; + const float target_y = data.path.y(target_idx) + signed_offset * perp_y; + const int last_idx = data.trajectories.y.cols() - 1; + const auto diff_x = target_x - data.trajectories.x.col(last_idx); + const auto diff_y = target_y - data.trajectories.y.col(last_idx); + + if (power_ > 1u) { + data.costs += + (((diff_x.square() + diff_y.square()).sqrt()) * weight_).pow(power_); + } else { + data.costs += ((diff_x.square() + diff_y.square()).sqrt()) * weight_; + } + + bypass_active_ = true; +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::ObstacleBypassCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index cfcb7614273..c9a2379b032 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -23,6 +23,7 @@ void PathAlignCritic::initialize() auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 10.0f); + getParam(min_distance_occupancy_check_, "min_distance_occupancy_check", 2.0f); getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07f); getParam(offset_from_furthest_, "offset_from_furthest", 20); getParam(trajectory_point_step_, "trajectory_point_step", 4); @@ -45,29 +46,15 @@ void PathAlignCritic::score(CriticData & data) // Don't apply when first getting bearing w.r.t. the path utils::setPathFurthestPointIfNotSet(data); - // Up to furthest only, closest path point is always 0 from path handler - const size_t path_segments_count = *data.furthest_reached_path_point; - float path_segments_flt = static_cast(path_segments_count); - if (path_segments_count < offset_from_furthest_) { + const size_t furthest_reached_path_point = *data.furthest_reached_path_point; + if (furthest_reached_path_point < offset_from_furthest_) { return; } - // Don't apply when dynamic obstacles are blocking significant proportions of the local path - utils::setPathCostsIfNotSet(data, costmap_ros_); - std::vector & path_pts_valid = *data.path_pts_valid; - float invalid_ctr = 0.0f; - for (size_t i = 0; i < path_segments_count; i++) { - if (!path_pts_valid[i]) {invalid_ctr += 1.0f;} - if (invalid_ctr / path_segments_flt > max_path_occupancy_ratio_ && invalid_ctr > 2.0f) { - return; - } - } - - const size_t batch_size = data.trajectories.x.rows(); - Eigen::ArrayXf cost(data.costs.rows()); - cost.setZero(); - - // Find integrated distance in the path + // Find integrated distance in the path and find the first path IDX further than + // max(min_distance_occupancy_check_, furthest_reached_path_point) + const size_t path_segments_count = data.path.x.size() - 1; + size_t occupancy_check_distance_idx = 0; std::vector path_integrated_distances(path_segments_count, 0.0f); std::vector path(path_segments_count); float dx = 0.0f, dy = 0.0f; @@ -80,6 +67,27 @@ void PathAlignCritic::score(CriticData & data) dx = data.path.x(i) - pose.x; dy = data.path.y(i) - pose.y; path_integrated_distances[i] = path_integrated_distances[i - 1] + sqrtf(dx * dx + dy * dy); + + if (path_integrated_distances[i] <= min_distance_occupancy_check_ || + i < furthest_reached_path_point) + { + occupancy_check_distance_idx = (i + 1 < path_segments_count) ? i + 1 : i; + } + } + + // Don't apply when dynamic obstacles are blocking significant proportions of the local path + const float occupancy_check_distance_idx_flt = static_cast(occupancy_check_distance_idx); + utils::setPathCostsIfNotSet(data, costmap_ros_); + std::vector & path_pts_valid = *data.path_pts_valid; + float invalid_ctr = 0.0f; + for (size_t i = 0; i < occupancy_check_distance_idx; i++) { + if (!path_pts_valid[i]) {invalid_ctr += 1.0f;} + if (invalid_ctr / occupancy_check_distance_idx_flt > max_path_occupancy_ratio_ && + invalid_ctr > 2.0f) + { + // Path is blocked, defer to ObstacleBypassCritic if desired to go around dynamically + return; + } } // Finish populating the path vector @@ -111,6 +119,10 @@ void PathAlignCritic::score(CriticData & data) Eigen::Stride<-1, -1>(outer_stride, 1)); const auto traj_sampled_size = T_x.cols(); + const size_t batch_size = data.trajectories.x.rows(); + Eigen::ArrayXf cost(data.costs.rows()); + cost.setZero(); + for (size_t t = 0; t < batch_size; ++t) { summed_path_dist = 0.0f; num_samples = 0u; @@ -118,6 +130,10 @@ void PathAlignCritic::score(CriticData & data) path_pt = 0u; float Tx_m1 = T_x(t, 0); float Ty_m1 = T_y(t, 0); + // At each (strided) traj point, find the path point whose integrated distance along + // the path is closest to the trajectory point's integrated distance along the trajectory. + // if that path point is not in collision, compute the Euclidean distance between the matching + // path pt & traj pt the total cost is the average of those distances across the trajectory for (int p = 1; p < traj_sampled_size; p++) { const float Tx = T_x(t, p); const float Ty = T_y(t, p); diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index abebfe18253..5337e33eeef 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -26,6 +26,7 @@ #include "nav2_mppi_controller/critics/goal_critic.hpp" #include "nav2_mppi_controller/critics/obstacles_critic.hpp" #include "nav2_mppi_controller/critics/cost_critic.hpp" +#include "nav2_mppi_controller/critics/obstacle_bypass_critic.hpp" #include "nav2_mppi_controller/critics/path_align_critic.hpp" #include "nav2_mppi_controller/critics/path_angle_critic.hpp" #include "nav2_mppi_controller/critics/path_follow_critic.hpp" @@ -928,3 +929,197 @@ TEST(CriticTests, VelocityDeadbandCritic) // 35.0 weight * 0.1 model_dt * (0.07 + 0.06 + 0.059) * 30 timesteps = 56.7 EXPECT_NEAR(costs(1), 19.845, 0.01); } + +TEST(CriticTests, ObstacleBypassCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + ParametersHandler param_handler(node, name); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + state.reset(1000, 30); + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + geometry_msgs::msg::Pose goal; + // 70-point path so target_idx (45) < path_segments_count (69) avoids zero tangent + path.reset(70); + for (int i = 0; i < 70; ++i) { + path.x(i) = 0.5f + i * 0.1f; + path.y(i) = 2.5f; + } + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt, {}}; + data.motion_model = std::make_shared(); + + // Obstacle at cells [25,30]x[22,27] = world [2.5,3.0]x[2.2,2.7] + // Path at y=2.5 (cell_y=25) passes through obstacle. Indices 20-25 are blocked. + // Perpendicular scan from obstacle: left(y+) free at s=3, right(y-) free at s=4 + auto * costmap = costmap_ros->getCostmap(); + for (unsigned int i = 25; i <= 30; ++i) { + for (unsigned int j = 22; j <= 27; ++j) { + costmap->setCost(i, j, nav2_costmap_2d::LETHAL_OBSTACLE); + } + } + + // Initialization + ObstacleBypassCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // -- Scenario 1: Early exit - path too short -- + state.local_path_length = 0.3f; + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + + // -- Scenario 2: Early exit - not advanced enough on path -- + state.local_path_length = 6.9f; + data.furthest_reached_path_point = 10; + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + + // -- Scenario 3: No obstacles - clear path (!path_blocked && !bypass_active_) -- + data.furthest_reached_path_point = 25; + data.path_pts_valid = std::vector(69, true); + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + + // -- Scenario 4: Path blocked - left side bypass with exact costs -- + // findPathCosts marks indices 20-25 invalid from costmap. 5 counted (0..24). + // occupancy_ratio=5/25=0.2, path_blocked=true + // blocked_idx=20, resume_idx=26, obstacle_idx=23, path_yaw=0 + // determineBestBypassSide: left preferred (fewer lethal+inflated cells above path) + // signed_offset = +(4*0.1+1.0) = +1.4, target_idx=45, target=(5.0, 3.9) + data.path_pts_valid = std::nullopt; + data.furthest_reached_path_point = 25; + generated_trajectories.x.col(29).setConstant(5.0f); + generated_trajectories.y.col(29).setConstant(3.9f); + generated_trajectories.x(1, 29) = 5.0f; generated_trajectories.y(1, 29) = 2.9f; + generated_trajectories.x(2, 29) = 4.0f; generated_trajectories.y(2, 29) = 3.9f; + generated_trajectories.x(3, 29) = 5.0f; generated_trajectories.y(3, 29) = 1.9f; + + critic.score(data); + EXPECT_NEAR(costs(0), 0.0, 1e-2); + EXPECT_NEAR(costs(1), 4.667, 0.02); // weight * 1.0m + EXPECT_NEAR(costs(2), 4.667, 0.02); // weight * 1.0m + EXPECT_NEAR(costs(3), 9.333, 0.02); // weight * 2.0m + EXPECT_LT(costs(1), costs(3)); + costs.setZero(); + + // -- Scenario 5: Hysteresis keeps bypass active -- + // bypass_active_=true from Scenario 4. Pre-set 2 invalid pts: ratio=2/25=0.08 + // path_blocked=(0.08>0.07 && 2>2.0)=false. 0.08 >= 0.035 → bypass stays active. + // Same obstacle in costmap → same bypass side/offset → same target (5.0, 3.9) + data.path_pts_valid = std::vector(69, true); + (*data.path_pts_valid)[22] = false; + (*data.path_pts_valid)[23] = false; + data.furthest_reached_path_point = 25; + + critic.score(data); + EXPECT_GT(costs.sum(), 0.0); + EXPECT_NEAR(costs(1), 4.667, 0.02); // traj 1 is 1.0m from target + costs.setZero(); + + // -- Scenario 6: Hysteresis deactivation -- + // bypass_active_=true. All valid: ratio=0.0 < 0.035 → deactivates, returns + data.path_pts_valid = std::vector(69, true); + data.furthest_reached_path_point = 25; + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + + // -- Scenario 7: Power parameter, power=2 -- + node->set_parameter(rclcpp::Parameter("critic.cost_power", 2)); + critic = ObstacleBypassCritic(); + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + data.path_pts_valid = std::nullopt; + data.furthest_reached_path_point = 25; + critic.score(data); + EXPECT_NEAR(costs(0), 0.0, 1e-2); + EXPECT_NEAR(costs(1), 21.78, 0.1); // (weight * 1.0)^2 + EXPECT_NEAR(costs(3), 87.11, 0.1); // (weight * 2.0)^2 + costs.setZero(); + + // -- Scenario 8: Right side preferred bypass -- + // Extend obstacle to [25,30]x[22,29]: more cells above path than below + // Right side has fewer lethal cells → right preferred + // Verify by checking trajectory below path costs less than trajectory above + for (unsigned int i = 25; i <= 30; ++i) { + for (unsigned int j = 28; j <= 29; ++j) { + costmap->setCost(i, j, nav2_costmap_2d::LETHAL_OBSTACLE); + } + } + node->set_parameter(rclcpp::Parameter("critic.cost_power", 1)); + critic = ObstacleBypassCritic(); + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + data.path_pts_valid = std::nullopt; + data.furthest_reached_path_point = 25; + generated_trajectories.x.col(29).setConstant(5.0f); + generated_trajectories.y(0, 29) = 0.5f; // far below path (toward right bypass) + generated_trajectories.y(1, 29) = 4.5f; // far above path (away from right bypass) + + critic.score(data); + EXPECT_GT(costs.sum(), 0.0); + EXPECT_LT(costs(0), costs(1)); // below path (right side) is closer to target + costs.setZero(); + + // Restore obstacle to original [25,30]x[22,27] + for (unsigned int i = 25; i <= 30; ++i) { + for (unsigned int j = 28; j <= 29; ++j) { + costmap->setCost(i, j, nav2_costmap_2d::FREE_SPACE); + } + } + + // -- Scenario 9: Blocked to end of path -- + // resume_idx scans to end without finding valid → returns early + critic = ObstacleBypassCritic(); + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + data.path_pts_valid = std::vector(69, true); + for (int i = 20; i < 69; ++i) { + (*data.path_pts_valid)[i] = false; + } + data.furthest_reached_path_point = 25; + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + + // -- Scenario 10: Degenerate tangent (tangent_len < 1e-6) -- + critic = ObstacleBypassCritic(); + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + float saved_x22 = path.x(22); + float saved_x23 = path.x(23); + path.x(22) = 2.75f; + path.x(23) = 2.75f; + data.path_pts_valid = std::vector(69, true); + (*data.path_pts_valid)[21] = false; + (*data.path_pts_valid)[22] = false; + (*data.path_pts_valid)[23] = false; + data.furthest_reached_path_point = 25; + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + path.x(22) = saved_x22; + path.x(23) = saved_x23; + + // -- Scenario 11: Both sides blocked (determineBestBypassSide returns false) -- + for (unsigned int i = 0; i < costmap->getSizeInCellsX(); ++i) { + for (unsigned int j = 0; j < costmap->getSizeInCellsY(); ++j) { + costmap->setCost(i, j, nav2_costmap_2d::LETHAL_OBSTACLE); + } + } + critic = ObstacleBypassCritic(); + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + data.path_pts_valid = std::vector(69, true); + for (int i = 20; i <= 25; ++i) { + (*data.path_pts_valid)[i] = false; + } + data.furthest_reached_path_point = 25; + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); +}