|
| 1 | +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov |
| 2 | +// Copyright (c) 2023 Open Navigation LLC |
| 3 | +// |
| 4 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 5 | +// you may not use this file except in compliance with the License. |
| 6 | +// You may obtain a copy of the License at |
| 7 | +// |
| 8 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 9 | +// |
| 10 | +// Unless required by applicable law or agreed to in writing, software |
| 11 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 12 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 13 | +// See the License for the specific language governing permissions and |
| 14 | +// limitations under the License. |
| 15 | + |
| 16 | +#include <cmath> |
| 17 | +#include "nav2_mppi_controller/critics/cost_critic.hpp" |
| 18 | + |
| 19 | +namespace mppi::critics |
| 20 | +{ |
| 21 | + |
| 22 | +void CostCritic::initialize() |
| 23 | +{ |
| 24 | + auto getParam = parameters_handler_->getParamGetter(name_); |
| 25 | + getParam(consider_footprint_, "consider_footprint", false); |
| 26 | + getParam(power_, "cost_power", 1); |
| 27 | + getParam(weight_, "cost_weight", 3.81); |
| 28 | + getParam(critical_cost_, "critical_cost", 300.0); |
| 29 | + getParam(collision_cost_, "collision_cost", 1000000.0); |
| 30 | + getParam(near_goal_distance_, "near_goal_distance", 0.5); |
| 31 | + getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); |
| 32 | + |
| 33 | + // Normalized by cost value to put in same regime as other weights |
| 34 | + weight_ /= 254.0f; |
| 35 | + |
| 36 | + collision_checker_.setCostmap(costmap_); |
| 37 | + possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); |
| 38 | + |
| 39 | + if (possibly_inscribed_cost_ < 1.0f) { |
| 40 | + RCLCPP_ERROR( |
| 41 | + logger_, |
| 42 | + "Inflation layer either not found or inflation is not set sufficiently for " |
| 43 | + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" |
| 44 | + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " |
| 45 | + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" |
| 46 | + " for full instructions. This will substantially impact run-time performance."); |
| 47 | + } |
| 48 | + |
| 49 | + RCLCPP_INFO( |
| 50 | + logger_, |
| 51 | + "InflationCostCritic instantiated with %d power and %f / %f weights. " |
| 52 | + "Critic will collision check based on %s cost.", |
| 53 | + power_, critical_cost_, weight_, consider_footprint_ ? |
| 54 | + "footprint" : "circular"); |
| 55 | +} |
| 56 | + |
| 57 | +float CostCritic::findCircumscribedCost( |
| 58 | + std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap) |
| 59 | +{ |
| 60 | + double result = -1.0; |
| 61 | + const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); |
| 62 | + if (static_cast<float>(circum_radius) == circumscribed_radius_) { |
| 63 | + // early return if footprint size is unchanged |
| 64 | + return circumscribed_cost_; |
| 65 | + } |
| 66 | + |
| 67 | + // check if the costmap has an inflation layer |
| 68 | + const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer( |
| 69 | + costmap, |
| 70 | + inflation_layer_name_); |
| 71 | + if (inflation_layer != nullptr) { |
| 72 | + const double resolution = costmap->getCostmap()->getResolution(); |
| 73 | + result = inflation_layer->computeCost(circum_radius / resolution); |
| 74 | + } else { |
| 75 | + RCLCPP_WARN( |
| 76 | + logger_, |
| 77 | + "No inflation layer found in costmap configuration. " |
| 78 | + "If this is an SE2-collision checking plugin, it cannot use costmap potential " |
| 79 | + "field to speed up collision checking by only checking the full footprint " |
| 80 | + "when robot is within possibly-inscribed radius of an obstacle. This may " |
| 81 | + "significantly slow down planning times and not avoid anything but absolute collisions!"); |
| 82 | + } |
| 83 | + |
| 84 | + circumscribed_radius_ = static_cast<float>(circum_radius); |
| 85 | + circumscribed_cost_ = static_cast<float>(result); |
| 86 | + |
| 87 | + return circumscribed_cost_; |
| 88 | +} |
| 89 | + |
| 90 | +void CostCritic::score(CriticData & data) |
| 91 | +{ |
| 92 | + using xt::evaluation_strategy::immediate; |
| 93 | + if (!enabled_) { |
| 94 | + return; |
| 95 | + } |
| 96 | + |
| 97 | + if (consider_footprint_) { |
| 98 | + // footprint may have changed since initialization if user has dynamic footprints |
| 99 | + possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); |
| 100 | + } |
| 101 | + |
| 102 | + // If near the goal, don't apply the preferential term since the goal is near obstacles |
| 103 | + bool near_goal = false; |
| 104 | + if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) { |
| 105 | + near_goal = true; |
| 106 | + } |
| 107 | + |
| 108 | + auto && repulsive_cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)}); |
| 109 | + repulsive_cost.fill(0.0); |
| 110 | + |
| 111 | + const size_t traj_len = data.trajectories.x.shape(1); |
| 112 | + bool all_trajectories_collide = true; |
| 113 | + for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) { |
| 114 | + bool trajectory_collide = false; |
| 115 | + const auto & traj = data.trajectories; |
| 116 | + float pose_cost; |
| 117 | + |
| 118 | + for (size_t j = 0; j < traj_len; j++) { |
| 119 | + // The costAtPose doesn't use orientation |
| 120 | + // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it |
| 121 | + // So the center point has more information than the footprint |
| 122 | + pose_cost = costAtPose(traj.x(i, j), traj.y(i, j)); |
| 123 | + if (pose_cost < 1.0f) {continue;} // In free space |
| 124 | + |
| 125 | + if (inCollision(pose_cost, traj.x(i, j), traj.y(i, j), traj.yaws(i, j))) { |
| 126 | + trajectory_collide = true; |
| 127 | + break; |
| 128 | + } |
| 129 | + |
| 130 | + // Let near-collision trajectory points be punished severely |
| 131 | + // Note that we collision check based on the footprint actual, |
| 132 | + // but score based on the center-point cost regardless |
| 133 | + using namespace nav2_costmap_2d; // NOLINT |
| 134 | + if (pose_cost >= INSCRIBED_INFLATED_OBSTACLE) { |
| 135 | + repulsive_cost[i] += critical_cost_; |
| 136 | + } else if (!near_goal) { // Generally prefer trajectories further from obstacles |
| 137 | + repulsive_cost[i] += pose_cost; |
| 138 | + } |
| 139 | + } |
| 140 | + |
| 141 | + if (!trajectory_collide) { |
| 142 | + all_trajectories_collide = false; |
| 143 | + } else { |
| 144 | + repulsive_cost[i] = collision_cost_; |
| 145 | + } |
| 146 | + } |
| 147 | + |
| 148 | + data.costs += xt::pow((weight_ * repulsive_cost / traj_len), power_); |
| 149 | + data.fail_flag = all_trajectories_collide; |
| 150 | +} |
| 151 | + |
| 152 | +/** |
| 153 | + * @brief Checks if cost represents a collision |
| 154 | + * @param cost Costmap cost |
| 155 | + * @return bool if in collision |
| 156 | + */ |
| 157 | +bool CostCritic::inCollision(float cost, float x, float y, float theta) |
| 158 | +{ |
| 159 | + bool is_tracking_unknown = |
| 160 | + costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); |
| 161 | + |
| 162 | + // If consider_footprint_ check footprint scort for collision |
| 163 | + if (consider_footprint_ && |
| 164 | + (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f)) |
| 165 | + { |
| 166 | + cost = static_cast<float>(collision_checker_.footprintCostAtPose( |
| 167 | + x, y, theta, costmap_ros_->getRobotFootprint())); |
| 168 | + } |
| 169 | + |
| 170 | + switch (static_cast<unsigned char>(cost)) { |
| 171 | + using namespace nav2_costmap_2d; // NOLINT |
| 172 | + case (LETHAL_OBSTACLE): |
| 173 | + return true; |
| 174 | + case (INSCRIBED_INFLATED_OBSTACLE): |
| 175 | + return consider_footprint_ ? false : true; |
| 176 | + case (NO_INFORMATION): |
| 177 | + return is_tracking_unknown ? false : true; |
| 178 | + } |
| 179 | + |
| 180 | + return false; |
| 181 | +} |
| 182 | + |
| 183 | +float CostCritic::costAtPose(float x, float y) |
| 184 | +{ |
| 185 | + using namespace nav2_costmap_2d; // NOLINT |
| 186 | + unsigned int x_i, y_i; |
| 187 | + if (!collision_checker_.worldToMap(x, y, x_i, y_i)) { |
| 188 | + return nav2_costmap_2d::NO_INFORMATION; |
| 189 | + } |
| 190 | + |
| 191 | + return collision_checker_.pointCost(x_i, y_i); |
| 192 | +} |
| 193 | + |
| 194 | +} // namespace mppi::critics |
| 195 | + |
| 196 | +#include <pluginlib/class_list_macros.hpp> |
| 197 | + |
| 198 | +PLUGINLIB_EXPORT_CLASS( |
| 199 | + mppi::critics::CostCritic, |
| 200 | + mppi::critics::CriticFunction) |
0 commit comments