From 4385a2d5f4a7ff0c43b1cc9653e3e89bfe22282f Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Aug 2025 15:29:58 +0200 Subject: [PATCH 1/5] Cost critic optimizations Signed-off-by: Tony Najjar --- nav2_mppi_controller/CMakeLists.txt | 2 + .../collision_checker.hpp | 159 +++++++ .../critics/cost_critic.hpp | 49 +-- .../src/collision_checker.cpp | 391 ++++++++++++++++++ .../src/critics/cost_critic.cpp | 132 +++--- 5 files changed, 603 insertions(+), 130 deletions(-) create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp create mode 100644 nav2_mppi_controller/src/collision_checker.cpp diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 5197a8859de..a2b31e9a437 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -39,6 +39,7 @@ endif() add_library(mppi_controller SHARED src/controller.cpp + src/collision_checker.cpp src/critic_manager.cpp src/noise_generator.cpp src/optimizer.cpp @@ -88,6 +89,7 @@ target_include_directories(mppi_critics "$" "$") target_link_libraries(mppi_critics PUBLIC + mppi_controller angles::angles ${geometry_msgs_TARGETS} nav2_core::nav2_core diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp new file mode 100644 index 00000000000..07e62c9ba7f --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp @@ -0,0 +1,159 @@ +// Copyright (c) 2025, Angsa Robotics GmbH +// +// 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. Reserved. + +#ifndef NAV2_MPPI_CONTROLLER__COLLISION_CHECKER_HPP_ +#define NAV2_MPPI_CONTROLLER__COLLISION_CHECKER_HPP_ + +#include +#include + +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +const float UNKNOWN_COST = 255.0; +const float OCCUPIED_COST = 254.0; +const float INSCRIBED_COST = 253.0; +const float MAX_NON_OBSTACLE_COST = 252.0; +const float FREE_COST = 0; + +namespace nav2_mppi_controller +{ + +/** + * @struct CollisionResult + * @brief Result structure for batch collision checking + */ +enum class CollisionType +{ + NONE = 0, + POINT_COST = 1, + FOOTPRINT_COST = 2, + SWEPT_AREA_COST = 3 +}; + +struct CollisionResult +{ + bool in_collision; + std::vector center_cost; + std::vector footprint_cost; +}; + +/** + * @class nav2_mppi_controller::MPPICollisionChecker + * @brief A costmap grid collision checker + */ +class MPPICollisionChecker + : public nav2_costmap_2d::FootprintCollisionChecker +{ +public: + /** + * @brief A constructor for nav2_mppi_controller::MPPICollisionChecker + * @param costmap The costmap to collision check against + * @param node Node to extract clock and logger from + */ + MPPICollisionChecker( + std::shared_ptr costmap, + rclcpp_lifecycle::LifecycleNode::SharedPtr node); + + /** + * @brief A constructor for nav2_mppi_controller::MPPICollisionChecker + * for use when irregular bin intervals are appropriate + * @param costmap The costmap to collision check against + * @param angles The vector of possible angle bins to precompute for + * orientations for to speed up collision checking, in radians + */ + // MPPICollisionChecker( + // nav2_costmap_2d::Costmap2D * costmap, + // std::vector & angles); + + /** + * @brief Set the footprint to use with collision checker + * @param footprint The footprint to collision check against + * @param radius Whether or not the footprint is a circle and use radius collision checking + * @param inflation_layer_name Optional name of inflation layer for cost calculation + */ + void setFootprint( + const nav2_costmap_2d::Footprint & footprint, + const bool & radius, + const std::string & inflation_layer_name = ""); + + /** + * @brief Check if in collision with costmap and footprint at poses (batch processing) + * @param x Vector of X coordinates of poses to check against + * @param y Vector of Y coordinates of poses to check against + * @param yaw Vector of yaw angles of poses to check against in radians + * @param traverse_unknown Whether or not to traverse in unknown space + * @return CollisionResult struct with collision status and vectors of center cost, footprint cost, and area cost + */ + CollisionResult inCollision( + const std::vector & x, + const std::vector & y, + const std::vector & yaw, + const bool & traverse_unknown); + + /** + * @brief Get costmap ros object for inflation layer params + * @return Costmap ros + */ + std::shared_ptr getCostmapROS() {return costmap_ros_;} + + /** + * @brief Check if value outside the range + * @param max Maximum value of the range + * @param value the value to check if it is within the range + * @return boolean if in range or not + */ + bool outsideRange(const unsigned int & max, const float & value) const; + + /** + * @brief Create convex hull from a set of points + * @param points Input points to create convex hull from + * @return Convex hull as a footprint + */ + nav2_costmap_2d::Footprint createConvexHull( + const std::vector & points); + + /** + * @brief Find the circumscribed cost for collision checking optimization + * @param inflation_layer_name Optional name of inflation layer + * @return The circumscribed cost value + */ + float findCircumscribedCost(const std::string & inflation_layer_name = ""); + +private: + /** + * @brief Transform footprint to given orientation + * @param footprint The base footprint to transform + * @param yaw The yaw angle to transform to + * @return Transformed footprint + */ + nav2_costmap_2d::Footprint transformFootprint( + const nav2_costmap_2d::Footprint & footprint, + float yaw) const; + +protected: + std::shared_ptr costmap_ros_; + nav2_costmap_2d::Footprint unoriented_footprint_; + bool footprint_is_radius_{false}; + float possible_collision_cost_{-1}; + float circumscribed_radius_{-1.0f}; + float circumscribed_cost_{-1.0f}; + rclcpp::Logger logger_{rclcpp::get_logger("MPPICollisionChecker")}; + rclcpp::Clock::SharedPtr clock_; +}; + +} // namespace nav2_mppi_controller + +#endif // NAV2_MPPI_CONTROLLER__COLLISION_CHECKER_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp index 2646c98fd9e..2a8435cacf5 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp @@ -18,8 +18,8 @@ #include #include -#include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" +#include "nav2_mppi_controller/collision_checker.hpp" #include "nav2_mppi_controller/critic_function.hpp" #include "nav2_mppi_controller/models/state.hpp" @@ -48,47 +48,6 @@ class CostCritic : public CriticFunction void score(CriticData & data) override; protected: - /** - * @brief Checks if cost represents a collision - * @param cost Point cost at pose center - * @param x X of pose - * @param y Y of pose - * @param theta theta of pose - * @return bool if in collision - */ - inline bool inCollision(float cost, float x, float y, float theta) - { - // If consider_footprint_ check footprint scort for collision - float score_cost = cost; - if (consider_footprint_ && - (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f)) - { - score_cost = static_cast(collision_checker_.footprintCostAtPose( - static_cast(x), static_cast(y), static_cast(theta), - costmap_ros_->getRobotFootprint())); - } - - switch (static_cast(score_cost)) { - case (nav2_costmap_2d::LETHAL_OBSTACLE): - return true; - case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): - return consider_footprint_ ? false : true; - case (nav2_costmap_2d::NO_INFORMATION): - return is_tracking_unknown_ ? false : true; - } - - return false; - } - - /** - * @brief Find the min cost of the inflation decay function for which the robot MAY be - * in collision in any orientation - * @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation) - * @return double circumscribed cost, any higher than this and need to do full footprint collision checking - * since some element of the robot could be in collision - */ - inline float findCircumscribedCost(std::shared_ptr costmap); - /** * @brief An implementation of worldToMap fully using floats * @param wx Float world X coord @@ -123,14 +82,12 @@ class CostCritic : public CriticFunction return my * size_x_ + mx; } - nav2_costmap_2d::FootprintCollisionChecker - collision_checker_{nullptr}; + std::unique_ptr collision_checker_; float possible_collision_cost_; + bool consider_footprint_{true}; bool is_tracking_unknown_{true}; - float circumscribed_radius_{0.0f}; - float circumscribed_cost_{0.0f}; float collision_cost_{0.0f}; float critical_cost_{0.0f}; unsigned int near_collision_cost_{253}; diff --git a/nav2_mppi_controller/src/collision_checker.cpp b/nav2_mppi_controller/src/collision_checker.cpp new file mode 100644 index 00000000000..95abf2754ca --- /dev/null +++ b/nav2_mppi_controller/src/collision_checker.cpp @@ -0,0 +1,391 @@ +// Copyright (c) 2025, Angsa Robotics GmbH +// +// 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. Reserved. + +#include "nav2_mppi_controller/collision_checker.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" +#include +#include + +namespace nav2_mppi_controller +{ + +MPPICollisionChecker::MPPICollisionChecker( + std::shared_ptr costmap_ros, + rclcpp_lifecycle::LifecycleNode::SharedPtr node) +: FootprintCollisionChecker(costmap_ros ? costmap_ros->getCostmap() : nullptr) +{ + if (node) { + clock_ = node->get_clock(); + logger_ = node->get_logger(); + } + + if (costmap_ros) { + costmap_ros_ = costmap_ros; + } +} + +void MPPICollisionChecker::setFootprint( + const nav2_costmap_2d::Footprint & footprint, + const bool & radius, + const std::string & inflation_layer_name) +{ + // Calculate the circumscribed cost automatically + possible_collision_cost_ = findCircumscribedCost(inflation_layer_name); + + if (possible_collision_cost_ <= 0.0f) { + RCLCPP_ERROR_THROTTLE( + logger_, *clock_, 1000, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_mppi_controller#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + + footprint_is_radius_ = radius; + + // Use radius, no caching required + if (radius) { + return; + } + + // Store the unoriented footprint for on-the-fly transformation + unoriented_footprint_ = footprint; +} + +CollisionResult MPPICollisionChecker::inCollision( + const std::vector & x, + const std::vector & y, + const std::vector & yaw, + const bool & traverse_unknown) +{ + CollisionResult result; + result.in_collision = false; + + // Check if all vectors have the same size + if (x.size() != y.size() || x.size() != yaw.size()) { + result.in_collision = true; + return result; + } + + // Initialize result vectors + result.center_cost.resize(x.size(), -1.0f); + result.footprint_cost.resize(x.size(), -1.0f); + + // Step 1: Check all poses for bounds and get center costs + std::vector needs_footprint_check(x.size(), false); + + for (size_t i = 0; i < x.size(); ++i) { + // Check to make sure cell is inside the map + if (outsideRange(costmap_->getSizeInCellsX(), x[i]) || + outsideRange(costmap_->getSizeInCellsY(), y[i])) + { + result.in_collision = true; + return result; + } + + // Get center cost for this pose + float current_center_cost = static_cast(costmap_->getCost( + static_cast(x[i] + 0.5f), static_cast(y[i] + 0.5f))); + + result.center_cost[i] = current_center_cost; + + if (current_center_cost == UNKNOWN_COST && !traverse_unknown) { + result.in_collision = true; + return result; + } + + if (current_center_cost >= INSCRIBED_COST) { + result.in_collision = true; + return result; + } + + // For footprint-based collision checking, mark poses that need further checking + if (!footprint_is_radius_) { + // Skip if center cost is below collision threshold + if (current_center_cost >= possible_collision_cost_ || possible_collision_cost_ <= 0.0f) { + needs_footprint_check[i] = true; + } + } + } + + if (footprint_is_radius_) { + return result; // No further checks needed for radius-based checking + } + + // Step 2: If using footprint, check footprint costs for poses that need it + std::vector needs_swept_area_check(x.size(), false); + // Cache computed footprints to avoid recomputation in Step 4 + std::vector cached_footprints(x.size()); + + for (size_t i = 0; i < x.size(); ++i) { + // Skip poses that don't need footprint checking + if (!needs_footprint_check[i]) { + continue; + } + + // Transform footprint to current orientation and world position + double wx, wy; + costmap_->mapToWorld(static_cast(x[i]), static_cast(y[i]), wx, wy); + + // Transform the footprint to the given orientation + nav2_costmap_2d::Footprint oriented_footprint = transformFootprint(unoriented_footprint_, + yaw[i]); + + // Translate to world position + nav2_costmap_2d::Footprint current_footprint; + current_footprint.reserve(oriented_footprint.size()); + geometry_msgs::msg::Point new_pt; + for (unsigned int j = 0; j < oriented_footprint.size(); ++j) { + new_pt.x = wx + oriented_footprint[j].x; + new_pt.y = wy + oriented_footprint[j].y; + current_footprint.push_back(new_pt); + } + + // Cache the computed footprint for potential reuse in Step 4 + cached_footprints[i] = current_footprint; + + // Check footprint perimeter + float footprint_cost = static_cast(footprintCost(current_footprint, false)); + + // Store footprint cost in result + result.footprint_cost[i] = footprint_cost; + + if (footprint_cost == UNKNOWN_COST && !traverse_unknown) { + result.in_collision = true; + return result; + } + + if (footprint_cost >= OCCUPIED_COST) { + result.in_collision = true; + return result; + } + + // Mark for swept area checking if footprint cost is INSCRIBED_COST + if (footprint_cost == INSCRIBED_COST) { + needs_swept_area_check[i] = true; + } + } + + // Step 3: Check swept area for consecutive poses with footprint cost INSCRIBED_COST + // Find consecutive sequences of poses that need swept area checking + std::vector> consecutive_sequences; + std::vector current_sequence; + + for (size_t i = 0; i < x.size(); ++i) { + if (needs_swept_area_check[i]) { + current_sequence.push_back(i); + // Limit sequence to maximum of 5 poses to avoid an unprecise swept area check + if (current_sequence.size() >= 5) { + consecutive_sequences.push_back(current_sequence); + current_sequence.clear(); + } + } else { + if (!current_sequence.empty()) { + // Only add sequences with more than one pose + if (current_sequence.size() > 1) { + consecutive_sequences.push_back(current_sequence); + } + current_sequence.clear(); + } + } + } + + // Don't forget the last sequence if it ends at the last pose + if (!current_sequence.empty()) { + // Only add sequences with more than one pose + if (current_sequence.size() > 1) { + consecutive_sequences.push_back(current_sequence); + } + } + + // Step 4: Check swept area using convex hull for each consecutive sequence + for (const auto & sequence : consecutive_sequences) { + // Collect all footprint points from consecutive poses using cached footprints + std::vector all_points; + + for (size_t seq_idx : sequence) { + // Use cached footprint instead of recomputing + const nav2_costmap_2d::Footprint & current_footprint = cached_footprints[seq_idx]; + + for (const auto & footprint_pt : current_footprint) { + all_points.push_back(footprint_pt); + } + } + + // Create convex hull from all collected points + nav2_costmap_2d::Footprint convex_hull = createConvexHull(all_points); + + // Check swept area cost using full area checking + float swept_area_cost = static_cast(footprintCost(convex_hull, true)); + + if (swept_area_cost == UNKNOWN_COST && !traverse_unknown) { + result.in_collision = true; + return result; + } + + if (swept_area_cost >= OCCUPIED_COST) { + result.in_collision = true; + return result; + } + } + + return result; +} + +nav2_costmap_2d::Footprint MPPICollisionChecker::createConvexHull( + const std::vector & points) +{ + if (points.size() <= 1) { + return points; + } + + // Create a copy of points for sorting + std::vector sorted_points = points; + + // Sort points lexicographically (first by x, then by y) + std::sort(sorted_points.begin(), sorted_points.end(), + [](const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) { + return a.x < b.x || (a.x == b.x && a.y < b.y); + }); + + // Remove duplicate points + sorted_points.erase( + std::unique(sorted_points.begin(), sorted_points.end(), + [](const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) { + return std::abs(a.x - b.x) < 1e-9 && std::abs(a.y - b.y) < 1e-9; + }), + sorted_points.end()); + + if (sorted_points.size() <= 1) { + return sorted_points; + } + + // Andrew's monotone chain algorithm + std::vector hull; + + // Helper function to compute cross product + auto cross = [](const geometry_msgs::msg::Point & O, + const geometry_msgs::msg::Point & A, + const geometry_msgs::msg::Point & B) { + return (A.x - O.x) * (B.y - O.y) - (A.y - O.y) * (B.x - O.x); + }; + + // Build lower hull + for (size_t i = 0; i < sorted_points.size(); ++i) { + while (hull.size() >= 2 && cross(hull[hull.size() - 2], hull[hull.size() - 1], + sorted_points[i]) <= 0) + { + hull.pop_back(); + } + hull.push_back(sorted_points[i]); + } + + // Build upper hull + size_t t = hull.size() + 1; + for (int i = static_cast(sorted_points.size()) - 2; i >= 0; --i) { + while (hull.size() >= t && cross(hull[hull.size() - 2], hull[hull.size() - 1], + sorted_points[i]) <= 0) + { + hull.pop_back(); + } + hull.push_back(sorted_points[i]); + } + + // Remove last point because it's the same as the first one + if (hull.size() > 1) { + hull.pop_back(); + } + + return hull; +} + +bool MPPICollisionChecker::outsideRange(const unsigned int & max, const float & value) const +{ + return value < 0.0f || value > max; +} + +nav2_costmap_2d::Footprint MPPICollisionChecker::transformFootprint( + const nav2_costmap_2d::Footprint & footprint, + float yaw) const +{ + double sin_th = sin(yaw); + double cos_th = cos(yaw); + nav2_costmap_2d::Footprint oriented_footprint; + oriented_footprint.reserve(footprint.size()); + + geometry_msgs::msg::Point new_pt; + for (unsigned int i = 0; i < footprint.size(); ++i) { + new_pt.x = footprint[i].x * cos_th - footprint[i].y * sin_th; + new_pt.y = footprint[i].x * sin_th + footprint[i].y * cos_th; + oriented_footprint.push_back(new_pt); + } + + return oriented_footprint; +} + +float MPPICollisionChecker::findCircumscribedCost(const std::string & inflation_layer_name) +{ + if (!costmap_ros_) { + RCLCPP_ERROR(logger_, "Costmap ROS is not available for circumscribed cost calculation"); + return -1.0f; + } + + double result = -1.0; + const double circum_radius = costmap_ros_->getLayeredCostmap()->getCircumscribedRadius(); + + if (static_cast(circum_radius) == circumscribed_radius_) { + // early return if footprint size is unchanged + return circumscribed_cost_; + } + + // check if the costmap has an inflation layer + const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer( + costmap_ros_, + inflation_layer_name); + + if (inflation_layer != nullptr) { + const double resolution = costmap_ros_->getCostmap()->getResolution(); + double inflation_radius = inflation_layer->getInflationRadius(); + + if (inflation_radius < circum_radius) { + RCLCPP_ERROR( + logger_, + "The inflation radius (%f) is smaller than the circumscribed radius (%f) " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times!", + inflation_radius, circum_radius); + result = 0.0; + } else { + result = inflation_layer->computeCost(circum_radius / resolution); + } + } else { + RCLCPP_WARN( + logger_, + "No inflation layer found in costmap configuration. " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times and not avoid anything but absolute collisions!"); + } + + circumscribed_radius_ = static_cast(circum_radius); + circumscribed_cost_ = static_cast(result); + + return circumscribed_cost_; +} + +} // namespace nav2_mppi_controller diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index dedf910187e..b95adb2eb60 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -16,6 +16,8 @@ #include #include "nav2_mppi_controller/critics/cost_critic.hpp" #include "nav2_core/controller_exceptions.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "geometry_msgs/msg/point.hpp" namespace mppi::critics { @@ -46,18 +48,8 @@ void CostCritic::initialize() }; parameters_handler_->addParamCallback(name_ + ".cost_weight", weightDynamicCb); - collision_checker_.setCostmap(costmap_); - possible_collision_cost_ = findCircumscribedCost(costmap_ros_); - - if (possible_collision_cost_ < 1.0f) { - RCLCPP_ERROR( - logger_, - "Inflation layer either not found or inflation is not set sufficiently for " - "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" - " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " - "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" - " for full instructions. This will substantially impact run-time performance."); - } + collision_checker_ = std::make_unique( + costmap_ros_, parent_.lock()); if (costmap_ros_->getUseRadius() == consider_footprint_) { RCLCPP_WARN( @@ -83,52 +75,6 @@ void CostCritic::initialize() "footprint" : "circular"); } -float CostCritic::findCircumscribedCost( - std::shared_ptr costmap) -{ - double result = -1.0; - const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); - if (static_cast(circum_radius) == circumscribed_radius_) { - // early return if footprint size is unchanged - return circumscribed_cost_; - } - - // check if the costmap has an inflation layer - const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer( - costmap, - inflation_layer_name_); - if (inflation_layer != nullptr) { - const double resolution = costmap->getCostmap()->getResolution(); - double inflation_radius = inflation_layer->getInflationRadius(); - if (inflation_radius < circum_radius) { - RCLCPP_ERROR( - rclcpp::get_logger("computeCircumscribedCost"), - "The inflation radius (%f) is smaller than the circumscribed radius (%f) " - "If this is an SE2-collision checking plugin, it cannot use costmap potential " - "field to speed up collision checking by only checking the full footprint " - "when robot is within possibly-inscribed radius of an obstacle. This may " - "significantly slow down planning times!", - inflation_radius, circum_radius); - result = 0.0; - return result; - } - result = inflation_layer->computeCost(circum_radius / resolution); - } else { - RCLCPP_WARN( - logger_, - "No inflation layer found in costmap configuration. " - "If this is an SE2-collision checking plugin, it cannot use costmap potential " - "field to speed up collision checking by only checking the full footprint " - "when robot is within possibly-inscribed radius of an obstacle. This may " - "significantly slow down planning times and not avoid anything but absolute collisions!"); - } - - circumscribed_radius_ = static_cast(circum_radius); - circumscribed_cost_ = static_cast(result); - - return circumscribed_cost_; -} - void CostCritic::score(CriticData & data) { if (!enabled_) { @@ -139,19 +85,13 @@ void CostCritic::score(CriticData & data) // Setup cost information for various parts of the critic is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); - auto * costmap = collision_checker_.getCostmap(); + auto * costmap = collision_checker_->getCostmap(); origin_x_ = static_cast(costmap->getOriginX()); origin_y_ = static_cast(costmap->getOriginY()); resolution_ = static_cast(costmap->getResolution()); size_x_ = costmap->getSizeInCellsX(); size_y_ = costmap->getSizeInCellsY(); - if (consider_footprint_) { - // footprint may have changed since initialization if user has dynamic footprints - possible_collision_cost_ = findCircumscribedCost(costmap_ros_); - } - - // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, goal)) { near_goal = true; @@ -175,41 +115,65 @@ void CostCritic::score(CriticData & data) Eigen::Stride<-1, -1>>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + collision_checker_->setFootprint( + costmap_ros_->getRobotFootprint(), + costmap_ros_->getUseRadius(), + inflation_layer_name_); + + // Batch collision checking using new vectorized function for (int i = 0; i < strided_traj_rows; ++i) { bool trajectory_collide = false; - float pose_cost = 0.0f; float & traj_cost = repulsive_cost(i); + // Prepare vectors for batch collision checking + std::vector x_coords, y_coords, yaw_angles; + x_coords.reserve(strided_traj_cols); + y_coords.reserve(strided_traj_cols); + yaw_angles.reserve(strided_traj_cols); + + // Convert world coordinates to map coordinates and collect yaw angles for (int j = 0; j < strided_traj_cols; j++) { float Tx = traj_x(i, j); float Ty = traj_y(i, j); unsigned int x_i = 0u, y_i = 0u; - // The getCost doesn't use orientation - // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it - // So the center point has more information than the footprint if (!worldToMapFloat(Tx, Ty, x_i, y_i)) { - pose_cost = 255.0f; // NO_INFORMATION in float - } else { - pose_cost = static_cast(costmap->getCost(getIndex(x_i, y_i))); - if (pose_cost < 1.0f) { - continue; // In free space - } - } - - if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) { + // If any point is outside the map, mark trajectory as colliding traj_cost = collision_cost_; trajectory_collide = true; break; } + x_coords.push_back(static_cast(x_i)); + y_coords.push_back(static_cast(y_i)); + yaw_angles.push_back(traj_yaw(i, j)); + } + + // Skip batch collision checking if trajectory already marked as colliding + if (trajectory_collide) { + continue; + } + + // Perform batch collision checking using yaw angles + auto collision_result = collision_checker_->inCollision( + x_coords, y_coords, yaw_angles, is_tracking_unknown_); + + if (collision_result.in_collision) { + traj_cost = collision_cost_; + trajectory_collide = true; + } else { // Let near-collision trajectory points be punished severely - // Note that we collision check based on the footprint actual, - // but score based on the center-point cost regardless - if (pose_cost >= static_cast(near_collision_cost_)) { - traj_cost += critical_cost_; - } else if (!near_goal) { // Generally prefer trajectories further from obstacles - traj_cost += pose_cost; + for (size_t k = 0; k < collision_result.footprint_cost.size(); ++k) { + // For optimization purposes, we don't always have the footprint cost + // but we still use it when available to staw away from collision more conservatively + float cost = collision_result.footprint_cost[k] != -1.0f ? + collision_result.footprint_cost[k] : collision_result.center_cost[k]; + + if (cost >= static_cast(near_collision_cost_)) { + traj_cost += critical_cost_; + } else if (!near_goal) { // Generally prefer trajectories further from obstacles + traj_cost += cost; + } } } From ec56f0988e03d6f881bd4980f13f5b9ffe4989ff Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Aug 2025 15:48:32 +0200 Subject: [PATCH 2/5] fix Signed-off-by: Tony Najjar --- .../nav2_mppi_controller/collision_checker.hpp | 16 ++-------------- nav2_mppi_controller/src/collision_checker.cpp | 18 +++++++++--------- 2 files changed, 11 insertions(+), 23 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp index 07e62c9ba7f..b8627fd6bc9 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp @@ -20,29 +20,17 @@ #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/cost_values.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -const float UNKNOWN_COST = 255.0; -const float OCCUPIED_COST = 254.0; -const float INSCRIBED_COST = 253.0; -const float MAX_NON_OBSTACLE_COST = 252.0; -const float FREE_COST = 0; namespace nav2_mppi_controller { /** * @struct CollisionResult - * @brief Result structure for batch collision checking + * @brief Result of collision checking */ -enum class CollisionType -{ - NONE = 0, - POINT_COST = 1, - FOOTPRINT_COST = 2, - SWEPT_AREA_COST = 3 -}; - struct CollisionResult { bool in_collision; diff --git a/nav2_mppi_controller/src/collision_checker.cpp b/nav2_mppi_controller/src/collision_checker.cpp index 95abf2754ca..b6971c20e02 100644 --- a/nav2_mppi_controller/src/collision_checker.cpp +++ b/nav2_mppi_controller/src/collision_checker.cpp @@ -101,12 +101,12 @@ CollisionResult MPPICollisionChecker::inCollision( result.center_cost[i] = current_center_cost; - if (current_center_cost == UNKNOWN_COST && !traverse_unknown) { + if (current_center_cost == nav2_costmap_2d::NO_INFORMATION && !traverse_unknown) { result.in_collision = true; return result; } - if (current_center_cost >= INSCRIBED_COST) { + if (current_center_cost >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { result.in_collision = true; return result; } @@ -162,23 +162,23 @@ CollisionResult MPPICollisionChecker::inCollision( // Store footprint cost in result result.footprint_cost[i] = footprint_cost; - if (footprint_cost == UNKNOWN_COST && !traverse_unknown) { + if (footprint_cost == nav2_costmap_2d::NO_INFORMATION && !traverse_unknown) { result.in_collision = true; return result; } - if (footprint_cost >= OCCUPIED_COST) { + if (footprint_cost >= nav2_costmap_2d::LETHAL_OBSTACLE) { result.in_collision = true; return result; } - // Mark for swept area checking if footprint cost is INSCRIBED_COST - if (footprint_cost == INSCRIBED_COST) { + // Mark for swept area checking if footprint cost is INSCRIBED_INFLATED_OBSTACLE + if (footprint_cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { needs_swept_area_check[i] = true; } } - // Step 3: Check swept area for consecutive poses with footprint cost INSCRIBED_COST + // Step 3: Check swept area for consecutive poses with footprint cost INSCRIBED_INFLATED_OBSTACLE // Find consecutive sequences of poses that need swept area checking std::vector> consecutive_sequences; std::vector current_sequence; @@ -230,12 +230,12 @@ CollisionResult MPPICollisionChecker::inCollision( // Check swept area cost using full area checking float swept_area_cost = static_cast(footprintCost(convex_hull, true)); - if (swept_area_cost == UNKNOWN_COST && !traverse_unknown) { + if (swept_area_cost == nav2_costmap_2d::NO_INFORMATION && !traverse_unknown) { result.in_collision = true; return result; } - if (swept_area_cost >= OCCUPIED_COST) { + if (swept_area_cost >= nav2_costmap_2d::LETHAL_OBSTACLE) { result.in_collision = true; return result; } From 9bfd62bf54d6439b237e497a94da7a3d994f45b8 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Aug 2025 16:17:01 +0200 Subject: [PATCH 3/5] remove unecessary dependency Signed-off-by: Tony Najjar --- nav2_mppi_controller/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index a2b31e9a437..18135c69926 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -89,7 +89,6 @@ target_include_directories(mppi_critics "$" "$") target_link_libraries(mppi_critics PUBLIC - mppi_controller angles::angles ${geometry_msgs_TARGETS} nav2_core::nav2_core From b4a71730fcd7d4425f3022f2286c7c18be762fdd Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Aug 2025 16:23:30 +0200 Subject: [PATCH 4/5] fix lint Signed-off-by: Tony Najjar --- nav2_mppi_controller/src/critics/cost_critic.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index b95adb2eb60..2ef8562c2c0 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -166,7 +166,7 @@ void CostCritic::score(CriticData & data) for (size_t k = 0; k < collision_result.footprint_cost.size(); ++k) { // For optimization purposes, we don't always have the footprint cost // but we still use it when available to staw away from collision more conservatively - float cost = collision_result.footprint_cost[k] != -1.0f ? + float cost = collision_result.footprint_cost[k] != -1.0f ? collision_result.footprint_cost[k] : collision_result.center_cost[k]; if (cost >= static_cast(near_collision_cost_)) { From fe363c60d3e57b359308964c4ab838f025da58d8 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Aug 2025 16:45:21 +0200 Subject: [PATCH 5/5] linting Signed-off-by: Tony Najjar --- .../include/nav2_mppi_controller/collision_checker.hpp | 1 + .../include/nav2_mppi_controller/critics/cost_critic.hpp | 2 -- nav2_mppi_controller/src/collision_checker.cpp | 7 +++++-- nav2_mppi_controller/src/critics/cost_critic.cpp | 5 ++--- 4 files changed, 8 insertions(+), 7 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp index b8627fd6bc9..d0e27b8db11 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp index 2a8435cacf5..3ab81be850e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp @@ -84,8 +84,6 @@ class CostCritic : public CriticFunction std::unique_ptr collision_checker_; float possible_collision_cost_; - - bool consider_footprint_{true}; bool is_tracking_unknown_{true}; float collision_cost_{0.0f}; diff --git a/nav2_mppi_controller/src/collision_checker.cpp b/nav2_mppi_controller/src/collision_checker.cpp index b6971c20e02..e3f46e3b130 100644 --- a/nav2_mppi_controller/src/collision_checker.cpp +++ b/nav2_mppi_controller/src/collision_checker.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include "nav2_mppi_controller/collision_checker.hpp" -#include "nav2_costmap_2d/inflation_layer.hpp" #include #include +#include "nav2_mppi_controller/collision_checker.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" + + namespace nav2_mppi_controller { @@ -75,6 +77,7 @@ CollisionResult MPPICollisionChecker::inCollision( // Check if all vectors have the same size if (x.size() != y.size() || x.size() != yaw.size()) { + RCLCPP_ERROR(logger_, "Unexpected size of input vectors for collision checking"); result.in_collision = true; return result; } diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index 2ef8562c2c0..edba6677cda 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -120,7 +120,7 @@ void CostCritic::score(CriticData & data) costmap_ros_->getUseRadius(), inflation_layer_name_); - // Batch collision checking using new vectorized function + // Batch collision checking for (int i = 0; i < strided_traj_rows; ++i) { bool trajectory_collide = false; float & traj_cost = repulsive_cost(i); @@ -154,7 +154,7 @@ void CostCritic::score(CriticData & data) continue; } - // Perform batch collision checking using yaw angles + // Perform batch collision checking auto collision_result = collision_checker_->inCollision( x_coords, y_coords, yaw_angles, is_tracking_unknown_); @@ -162,7 +162,6 @@ void CostCritic::score(CriticData & data) traj_cost = collision_cost_; trajectory_collide = true; } else { - // Let near-collision trajectory points be punished severely for (size_t k = 0; k < collision_result.footprint_cost.size(); ++k) { // For optimization purposes, we don't always have the footprint cost // but we still use it when available to staw away from collision more conservatively