Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
Public Types | Public Member Functions | Protected Attributes | List of all members
mppi::OptimalTrajectoryValidator Class Reference

Abstract base class for validating optimal trajectories from MPPI optimization. More...

#include <nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp>

Collaboration diagram for mppi::OptimalTrajectoryValidator:
Collaboration graph
[legend]

Public Types

using Ptr = std::shared_ptr< OptimalTrajectoryValidator >
 

Public Member Functions

 OptimalTrajectoryValidator ()=default
 Constructor for mppi::OptimalTrajectoryValidator.
 
virtual ~OptimalTrajectoryValidator ()=default
 Destructor for mppi::OptimalTrajectoryValidator.
 
virtual void initialize (const nav2::LifecycleNode::WeakPtr &parent, const std::string &name, const std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmap, ParametersHandler *param_handler, const std::shared_ptr< tf2_ros::Buffer > tf_buffer, const models::OptimizerSettings &settings)
 Initialize the trajectory validator. More...
 
virtual ValidationResult validateTrajectory (const Eigen::ArrayXXf &optimal_trajectory, const models::ControlSequence &, const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path &, const geometry_msgs::msg::Pose &)
 Validate the optimal trajectory from MPPI optimization Could be used to check for collisions, progress towards goal, distance from path, min distance from obstacles, dynamic feasibility, etc. More...
 

Protected Attributes

nav2::LifecycleNode::WeakPtr node_
 
std::string name_
 
std::shared_ptr< nav2_costmap_2d::Costmap2DROScostmap_ros_
 
ParametersHandlerparam_handler_
 
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
 
float collision_lookahead_time_ {1.0f}
 
unsigned int traj_samples_to_evaluate_ {0u}
 
bool consider_footprint_ {false}
 
std::unique_ptr< nav2_costmap_2d::FootprintCollisionChecker< nav2_costmap_2d::Costmap2D * > > collision_checker_
 

Detailed Description

Abstract base class for validating optimal trajectories from MPPI optimization.

Definition at line 47 of file optimal_trajectory_validator.hpp.

Member Function Documentation

◆ initialize()

virtual void mppi::OptimalTrajectoryValidator::initialize ( const nav2::LifecycleNode::WeakPtr &  parent,
const std::string &  name,
const std::shared_ptr< nav2_costmap_2d::Costmap2DROS costmap,
ParametersHandler param_handler,
const std::shared_ptr< tf2_ros::Buffer >  tf_buffer,
const models::OptimizerSettings settings 
)
inlinevirtual

Initialize the trajectory validator.

Parameters
nodeWeak pointer to the lifecycle node
nameName of the validator plugin
costmapShared pointer to the costmap ROS wrapper
param_handlerPointer to the parameters handler
tf_bufferShared pointer to the TF buffer
settingsSettings for the MPPI optimizer

Definition at line 71 of file optimal_trajectory_validator.hpp.

References mppi::ParametersHandler::getParamGetter().

Here is the call graph for this function:

◆ validateTrajectory()

virtual ValidationResult mppi::OptimalTrajectoryValidator::validateTrajectory ( const Eigen::ArrayXXf &  optimal_trajectory,
const models::ControlSequence ,
const geometry_msgs::msg::PoseStamped &  ,
const geometry_msgs::msg::Twist &  ,
const nav_msgs::msg::Path &  ,
const geometry_msgs::msg::Pose &   
)
inlinevirtual

Validate the optimal trajectory from MPPI optimization Could be used to check for collisions, progress towards goal, distance from path, min distance from obstacles, dynamic feasibility, etc.

Parameters
optimal_trajectoryThe optimal trajectory to validate
control_sequenceThe control sequence to validate
robot_poseThe current pose of the robot
robot_speedThe current speed of the robot
planThe planned path for the robot
goalThe goal pose for the robot
Returns
True if the trajectory is valid, false otherwise

Definition at line 117 of file optimal_trajectory_validator.hpp.


The documentation for this class was generated from the following file: