Evaluates a Trajectory2D to produce a score. More...
#include <trajectory_critic.h>
Public Member Functions | |
| virtual void | addCriticVisualization (sensor_msgs::PointCloud &pc) |
| Add information to the given pointcloud for debugging costmap-grid based scores. | |
| virtual void | debrief (const nav_2d_msgs::Twist2D &cmd_vel) |
| debrief informs the critic what the chosen cmd_vel was (if it cares) | |
| std::string | getName () |
| virtual double | getScale () const |
| void | initialize (const ros::NodeHandle &planner_nh, std::string name, nav_core2::Costmap::Ptr costmap) |
| Initialize the critic with appropriate pointers and parameters. | |
| virtual void | onInit () |
| virtual bool | prepare (const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel, const geometry_msgs::Pose2D &goal, const nav_2d_msgs::Path2D &global_plan) |
| Prior to evaluating any trajectories, look at contextual information constant across all trajectories. | |
| virtual void | reset () |
| Reset the state of the critic. | |
| virtual double | scoreTrajectory (const dwb_msgs::Trajectory2D &traj)=0 |
| Return a raw score for the given trajectory. | |
| void | setScale (const double scale) |
| virtual | ~TrajectoryCritic () |
Protected Attributes | |
| nav_core2::Costmap::Ptr | costmap_ |
| ros::NodeHandle | critic_nh_ |
| std::string | name_ |
| ros::NodeHandle | planner_nh_ |
| double | scale_ |
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_local_planner which will publish the grid scores as a PointCloud2.
Definition at line 75 of file trajectory_critic.h.
| virtual dwb_local_planner::TrajectoryCritic::~TrajectoryCritic | ( | ) | [inline, virtual] |
Definition at line 80 of file trajectory_critic.h.
| virtual void dwb_local_planner::TrajectoryCritic::addCriticVisualization | ( | sensor_msgs::PointCloud & | pc | ) | [inline, virtual] |
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_local_planner 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.
| pc | PointCloud to add channels to |
Definition at line 158 of file trajectory_critic.h.
| virtual void dwb_local_planner::TrajectoryCritic::debrief | ( | const nav_2d_msgs::Twist2D & | cmd_vel | ) | [inline, virtual] |
debrief informs the critic what the chosen cmd_vel was (if it cares)
Definition at line 140 of file trajectory_critic.h.
| std::string dwb_local_planner::TrajectoryCritic::getName | ( | ) | [inline] |
Definition at line 160 of file trajectory_critic.h.
| virtual double dwb_local_planner::TrajectoryCritic::getScale | ( | ) | const [inline, virtual] |
Definition at line 165 of file trajectory_critic.h.
| void dwb_local_planner::TrajectoryCritic::initialize | ( | const ros::NodeHandle & | planner_nh, |
| std::string | name, | ||
| nav_core2::Costmap::Ptr | costmap | ||
| ) | [inline] |
Initialize the critic with appropriate pointers and parameters.
The name and costmap are stored as member variables. A NodeHandle for the critic is created with the namespace of the planner NodeHandle
| planner_nh | Planner Nodehandle |
| name | The name of this critic |
| costmap_ros | Pointer to the costmap |
Definition at line 92 of file trajectory_critic.h.
| virtual void dwb_local_planner::TrajectoryCritic::onInit | ( | ) | [inline, virtual] |
Definition at line 102 of file trajectory_critic.h.
| virtual bool dwb_local_planner::TrajectoryCritic::prepare | ( | const geometry_msgs::Pose2D & | pose, |
| const nav_2d_msgs::Twist2D & | vel, | ||
| const geometry_msgs::Pose2D & | goal, | ||
| const nav_2d_msgs::Path2D & | global_plan | ||
| ) | [inline, virtual] |
Prior to evaluating any trajectories, look at contextual information constant across all trajectories.
Subclasses may overwrite. Return false in case there is any error.
| 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 |
Definition at line 122 of file trajectory_critic.h.
| virtual void dwb_local_planner::TrajectoryCritic::reset | ( | ) | [inline, virtual] |
Reset the state of the critic.
Reset is called when the planner receives a new global goal. This can be used to discard information specific to one plan.
Definition at line 110 of file trajectory_critic.h.
| virtual double dwb_local_planner::TrajectoryCritic::scoreTrajectory | ( | const dwb_msgs::Trajectory2D & | traj | ) | [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.
| void dwb_local_planner::TrajectoryCritic::setScale | ( | const double | scale | ) | [inline] |
Definition at line 166 of file trajectory_critic.h.
nav_core2::Costmap::Ptr dwb_local_planner::TrajectoryCritic::costmap_ [protected] |
Definition at line 169 of file trajectory_critic.h.
Definition at line 171 of file trajectory_critic.h.
std::string dwb_local_planner::TrajectoryCritic::name_ [protected] |
Definition at line 168 of file trajectory_critic.h.
Definition at line 171 of file trajectory_critic.h.
double dwb_local_planner::TrajectoryCritic::scale_ [protected] |
Definition at line 170 of file trajectory_critic.h.