|
| 1 | +// Copyright (c) 2025 Open Navigation LLC |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#ifndef NAV2_MPPI_CONTROLLER__OPTIMAL_TRAJECTORY_VALIDATOR_HPP_ |
| 16 | +#define NAV2_MPPI_CONTROLLER__OPTIMAL_TRAJECTORY_VALIDATOR_HPP_ |
| 17 | + |
| 18 | +#include <Eigen/Core> |
| 19 | +#include <string> |
| 20 | +#include <memory> |
| 21 | + |
| 22 | +#include "nav2_ros_common/lifecycle_node.hpp" |
| 23 | +#include "nav2_costmap_2d/costmap_2d_ros.hpp" |
| 24 | +#include "tf2_ros/buffer.h" |
| 25 | +#include "geometry_msgs/msg/twist.hpp" |
| 26 | +#include "geometry_msgs/msg/pose_stamped.hpp" |
| 27 | +#include "nav_msgs/msg/path.hpp" |
| 28 | + |
| 29 | +#include "nav2_mppi_controller/tools/parameters_handler.hpp" |
| 30 | +#include "nav2_mppi_controller/models/control_sequence.hpp" |
| 31 | +#include "nav2_mppi_controller/models/optimizer_settings.hpp" |
| 32 | + |
| 33 | +namespace mppi |
| 34 | +{ |
| 35 | + |
| 36 | +enum class ValidationResult |
| 37 | +{ |
| 38 | + SUCCESS, |
| 39 | + SOFT_RESET, |
| 40 | + FAILURE, |
| 41 | +}; |
| 42 | + |
| 43 | +/** |
| 44 | + * @class mppi::OptimalTrajectoryValidator |
| 45 | + * @brief Abstract base class for validating optimal trajectories from MPPI optimization |
| 46 | + */ |
| 47 | +class OptimalTrajectoryValidator |
| 48 | +{ |
| 49 | +public: |
| 50 | + using Ptr = std::shared_ptr<OptimalTrajectoryValidator>; |
| 51 | + |
| 52 | + /** |
| 53 | + * @brief Constructor for mppi::OptimalTrajectoryValidator |
| 54 | + */ |
| 55 | + OptimalTrajectoryValidator() = default; |
| 56 | + |
| 57 | + /** |
| 58 | + * @brief Destructor for mppi::OptimalTrajectoryValidator |
| 59 | + */ |
| 60 | + virtual ~OptimalTrajectoryValidator() = default; |
| 61 | + |
| 62 | + /** |
| 63 | + * @brief Initialize the trajectory validator |
| 64 | + * @param node Weak pointer to the lifecycle node |
| 65 | + * @param name Name of the validator plugin |
| 66 | + * @param costmap Shared pointer to the costmap ROS wrapper |
| 67 | + * @param param_handler Pointer to the parameters handler |
| 68 | + * @param tf_buffer Shared pointer to the TF buffer |
| 69 | + * @param settings Settings for the MPPI optimizer |
| 70 | + */ |
| 71 | + virtual void initialize( |
| 72 | + const nav2::LifecycleNode::WeakPtr & parent, |
| 73 | + const std::string & name, |
| 74 | + const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap, |
| 75 | + ParametersHandler * param_handler, |
| 76 | + const std::shared_ptr<tf2_ros::Buffer> tf_buffer, |
| 77 | + const models::OptimizerSettings & settings) |
| 78 | + { |
| 79 | + param_handler_ = param_handler; |
| 80 | + name_ = name; |
| 81 | + node_ = parent; |
| 82 | + costmap_ros_ = costmap; |
| 83 | + tf_buffer_ = tf_buffer; |
| 84 | + |
| 85 | + auto node = node_.lock(); |
| 86 | + auto getParam = param_handler_->getParamGetter(name_); |
| 87 | + getParam(collision_lookahead_time_, "collision_lookahead_time", 2.0f); |
| 88 | + traj_samples_to_evaluate_ = collision_lookahead_time_ / settings.model_dt; |
| 89 | + if (traj_samples_to_evaluate_ > settings.time_steps) { |
| 90 | + traj_samples_to_evaluate_ = settings.time_steps; |
| 91 | + RCLCPP_WARN( |
| 92 | + node->get_logger(), |
| 93 | + "Collision lookahead time is greater than the number of trajectory samples, " |
| 94 | + "setting it to the maximum number of samples (%u).", |
| 95 | + traj_samples_to_evaluate_); |
| 96 | + } |
| 97 | + |
| 98 | + getParam(consider_footprint_, "consider_footprint", false); |
| 99 | + if (consider_footprint_) { |
| 100 | + collision_checker_ = std::make_unique<nav2_costmap_2d::FootprintCollisionChecker< |
| 101 | + nav2_costmap_2d::Costmap2D *>>(costmap_ros_->getCostmap()); |
| 102 | + } |
| 103 | + } |
| 104 | + |
| 105 | + /** |
| 106 | + * @brief Validate the optimal trajectory from MPPI optimization |
| 107 | + * Could be used to check for collisions, progress towards goal, |
| 108 | + * distance from path, min distance from obstacles, dynamic feasibility, etc. |
| 109 | + * @param optimal_trajectory The optimal trajectory to validate |
| 110 | + * @param control_sequence The control sequence to validate |
| 111 | + * @param robot_pose The current pose of the robot |
| 112 | + * @param robot_speed The current speed of the robot |
| 113 | + * @param plan The planned path for the robot |
| 114 | + * @param goal The goal pose for the robot |
| 115 | + * @return True if the trajectory is valid, false otherwise |
| 116 | + */ |
| 117 | + virtual ValidationResult validateTrajectory( |
| 118 | + const Eigen::ArrayXXf & optimal_trajectory, |
| 119 | + const models::ControlSequence & /*control_sequence*/, |
| 120 | + const geometry_msgs::msg::PoseStamped & /*robot_pose*/, |
| 121 | + const geometry_msgs::msg::Twist & /*robot_speed*/, |
| 122 | + const nav_msgs::msg::Path & /*plan*/, |
| 123 | + const geometry_msgs::msg::Pose & /*goal*/) |
| 124 | + { |
| 125 | + // The Optimizer automatically ensures that we are within Kinematic |
| 126 | + // and dynamic constraints, no need to check for those again. |
| 127 | + |
| 128 | + // Check for collisions. This is highly unlikely to occur since the Obstacle/Cost Critics |
| 129 | + // penalize collisions severely, but it is still possible if those critics are not used or the |
| 130 | + // optimized trajectory is very near obstacles and the dynamic constraints cause invalidity. |
| 131 | + auto costmap = costmap_ros_->getCostmap(); |
| 132 | + for (size_t i = 0; i < traj_samples_to_evaluate_; ++i) { |
| 133 | + const double x = static_cast<double>(optimal_trajectory(i, 0)); |
| 134 | + const double y = static_cast<double>(optimal_trajectory(i, 1)); |
| 135 | + |
| 136 | + if (consider_footprint_) { |
| 137 | + const double theta = static_cast<double>(optimal_trajectory(i, 2)); |
| 138 | + double footprint_cost = collision_checker_->footprintCostAtPose( |
| 139 | + x, y, theta, costmap_ros_->getRobotFootprint()); |
| 140 | + if (footprint_cost == static_cast<double>(nav2_costmap_2d::LETHAL_OBSTACLE)) { |
| 141 | + return ValidationResult::SOFT_RESET; |
| 142 | + } |
| 143 | + } else { |
| 144 | + unsigned int x_i = 0u, y_i = 0u; |
| 145 | + if (!costmap->worldToMap(x, y, x_i, y_i)) { |
| 146 | + continue; // Out of bounds, skip this point |
| 147 | + } |
| 148 | + unsigned char cost = costmap->getCost(x_i, y_i); |
| 149 | + if (cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE || |
| 150 | + cost == nav2_costmap_2d::LETHAL_OBSTACLE) |
| 151 | + { |
| 152 | + return ValidationResult::SOFT_RESET; |
| 153 | + } |
| 154 | + } |
| 155 | + } |
| 156 | + |
| 157 | + return ValidationResult::SUCCESS; |
| 158 | + } |
| 159 | + |
| 160 | +protected: |
| 161 | + nav2::LifecycleNode::WeakPtr node_; |
| 162 | + std::string name_; |
| 163 | + std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_; |
| 164 | + ParametersHandler * param_handler_; |
| 165 | + std::shared_ptr<tf2_ros::Buffer> tf_buffer_; |
| 166 | + float collision_lookahead_time_{1.0f}; |
| 167 | + unsigned int traj_samples_to_evaluate_{0u}; |
| 168 | + bool consider_footprint_{false}; |
| 169 | + std::unique_ptr<nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>> |
| 170 | + collision_checker_; |
| 171 | +}; |
| 172 | + |
| 173 | +} // namespace mppi |
| 174 | + |
| 175 | +#endif // NAV2_MPPI_CONTROLLER__OPTIMAL_TRAJECTORY_VALIDATOR_HPP_ |
0 commit comments