35 #ifndef DWB_LOCAL_PLANNER_TRAJECTORY_CRITIC_H 36 #define DWB_LOCAL_PLANNER_TRAJECTORY_CRITIC_H 41 #include <geometry_msgs/Pose2D.h> 42 #include <nav_2d_msgs/Twist2D.h> 43 #include <nav_2d_msgs/Path2D.h> 44 #include <dwb_msgs/Trajectory2D.h> 45 #include <sensor_msgs/PointCloud.h> 78 using Ptr = std::shared_ptr<dwb_local_planner::TrajectoryCritic>;
122 virtual bool prepare(
const geometry_msgs::Pose2D& pose,
const nav_2d_msgs::Twist2D& vel,
123 const geometry_msgs::Pose2D& goal,
124 const nav_2d_msgs::Path2D& global_plan)
135 virtual double scoreTrajectory(
const dwb_msgs::Trajectory2D& traj) = 0;
140 virtual void debrief(
const nav_2d_msgs::Twist2D& cmd_vel) {}
176 #endif // DWB_LOCAL_PLANNER_TRAJECTORY_CRITIC_H
virtual void reset()
Reset the state of the critic.
std::shared_ptr< dwb_local_planner::TrajectoryCritic > Ptr
virtual void addCriticVisualization(sensor_msgs::PointCloud &pc)
Add information to the given pointcloud for debugging costmap-grid based scores.
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 debrief(const nav_2d_msgs::Twist2D &cmd_vel)
debrief informs the critic what the chosen cmd_vel was (if it cares)
ros::NodeHandle critic_nh_
virtual double getScale() const
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
nav_core2::Costmap::Ptr costmap_
Evaluates a Trajectory2D to produce a score.
void initialize(const ros::NodeHandle &planner_nh, std::string name, nav_core2::Costmap::Ptr costmap)
Initialize the critic with appropriate pointers and parameters.
void setScale(const double scale)
ros::NodeHandle planner_nh_
virtual ~TrajectoryCritic()
std::shared_ptr< Costmap > Ptr
virtual double scoreTrajectory(const dwb_msgs::Trajectory2D &traj)=0
Return a raw score for the given trajectory.