Evaluates a Trajectory2D to produce a score. More...
#include <nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp>
Public Types | |
| using | Ptr = std::shared_ptr< dwb_core::TrajectoryCritic > |
Public Member Functions | |
| void | initialize (const nav2_util::LifecycleNode::SharedPtr &nh, const std::string &name, const std::string &ns, std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmap_ros) |
| Initialize the critic with appropriate pointers and parameters. More... | |
| virtual void | onInit () |
| virtual void | reset () |
| Reset the state of the critic. More... | |
| virtual bool | prepare (const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &, const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Path2D &) |
| Prior to evaluating any trajectories, look at contextual information constant across all trajectories. More... | |
| virtual double | scoreTrajectory (const dwb_msgs::msg::Trajectory2D &traj)=0 |
| Return a raw score for the given trajectory. More... | |
| virtual void | debrief (const nav_2d_msgs::msg::Twist2D &) |
| debrief informs the critic what the chosen cmd_vel was (if it cares) | |
| virtual void | addCriticVisualization (std::vector< std::pair< std::string, std::vector< float >>> &) |
| Add information to the given pointcloud for debugging costmap-grid based scores. More... | |
| std::string | getName () |
| virtual double | getScale () const |
| void | setScale (const double scale) |
Protected Attributes | |
| std::string | name_ |
| std::string | dwb_plugin_name_ |
| std::shared_ptr< nav2_costmap_2d::Costmap2DROS > | costmap_ros_ |
| double | scale_ |
| rclcpp_lifecycle::LifecycleNode::WeakPtr | node_ |
Detailed Description
Evaluates a Trajectory2D to produce a score.
This class defines the plugin interface for the TrajectoryCritic which gives scores to trajectories, where lower numbers are better, but negative scores are considered invalid.
The general lifecycle is 1) initialize is called once at the beginning which in turn calls onInit. Derived classes may override onInit to load parameters as needed. 2) prepare is called once before each set of trajectories. It is presumed that there are multiple trajectories that we want to evaluate, and there may be some shared work that can be done beforehand to optimize the scoring of each individual trajectory. 3) scoreTrajectory is called once per trajectory and returns the score. 4) debrief is called after each set of trajectories with the chosen trajectory. This can be used for stateful critics that monitor the trajectory through time.
Optionally, there is also a debugging mechanism for certain types of critics in the addCriticVisualization method. If the score for a trajectory depends on its relationship to the costmap, addCriticVisualization can provide that information to the dwb_core which will publish the grid scores as a PointCloud2.
Definition at line 78 of file trajectory_critic.hpp.
Member Function Documentation
◆ addCriticVisualization()
|
inlinevirtual |
Add information to the given pointcloud for debugging costmap-grid based scores.
addCriticVisualization is an optional debugging mechanism for providing rich information about the cost for certain trajectories. Some critics will have scoring mechanisms wherein there will be some score for each cell in the costmap. This could be as straightforward as the cost in the costmap, or it could be the number of cells away from the goal pose.
Prior to calling this, dwb_core will load the PointCloud's header and the points in row-major order. The critic may then add a ChannelFloat to the channels member of the PC with the same number of values as the points array. This information may then be converted and published as a PointCloud2.
- Parameters
-
pc PointCloud to add channels to
Reimplemented in dwb_critics::MapGridCritic, and dwb_critics::BaseObstacleCritic.
Definition at line 170 of file trajectory_critic.hpp.
◆ initialize()
|
inline |
Initialize the critic with appropriate pointers and parameters.
The name and costmap are stored as member variables. A NodeHandle is created using the combination of the parent namespace and the critic name
- Parameters
-
name The name of this critic parent_namespace The namespace of the planner costmap_ros Pointer to the costmap
Definition at line 95 of file trajectory_critic.hpp.
◆ prepare()
|
inlinevirtual |
Prior to evaluating any trajectories, look at contextual information constant across all trajectories.
Subclasses may overwrite. Return false in case there is any error.
- Parameters
-
pose Current pose (costmap frame) vel Current velocity goal The final goal (costmap frame) global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
Reimplemented in dwb_critics::RotateToGoalCritic, dwb_critics::PathDistCritic, dwb_critics::PathAlignCritic, dwb_critics::OscillationCritic, dwb_critics::ObstacleFootprintCritic, dwb_critics::GoalDistCritic, and dwb_critics::GoalAlignCritic.
Definition at line 133 of file trajectory_critic.hpp.
◆ reset()
|
inlinevirtual |
Reset the state of the critic.
Reset is called when the planner receives a new global plan. This can be used to discard information specific to one plan.
Reimplemented in dwb_critics::RotateToGoalCritic, dwb_critics::OscillationCritic, and dwb_critics::MapGridCritic.
Definition at line 121 of file trajectory_critic.hpp.
◆ scoreTrajectory()
|
pure virtual |
Return a raw score for the given trajectory.
scores < 0 are considered invalid/errors, such as collisions This is the raw score in that the scale should not be applied to it.
Implemented in dwb_critics::TwirlingCritic, dwb_critics::RotateToGoalCritic, dwb_critics::PreferForwardCritic, dwb_critics::OscillationCritic, dwb_critics::MapGridCritic, and dwb_critics::BaseObstacleCritic.
The documentation for this class was generated from the following file:
- nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp
