Scores trajectories based on how far along the global path they end up. More...
#include <goal_dist.h>
Public Member Functions | |
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) override |
![]() | |
void | addCriticVisualization (sensor_msgs::PointCloud &pc) override |
double | getScale () const override |
double | getScore (unsigned int x, unsigned int y) |
Retrieve the score for a particular cell of the costmap. More... | |
MapGridCritic () | |
void | onInit () override |
virtual double | scorePose (const geometry_msgs::Pose2D &pose) |
Retrieve the score for a single pose. More... | |
double | scoreTrajectory (const dwb_msgs::Trajectory2D &traj) override |
void | setAsObstacle (unsigned int x, unsigned int y) |
Sets the score of a particular cell to the obstacle cost. More... | |
![]() | |
virtual void | debrief (const nav_2d_msgs::Twist2D &cmd_vel) |
std::string | getName () |
void | initialize (const ros::NodeHandle &planner_nh, std::string name, nav_core2::Costmap::Ptr costmap) |
void | setScale (const double scale) |
virtual | ~TrajectoryCritic () |
Protected Member Functions | |
bool | getLastPoseOnCostmap (const nav_2d_msgs::Path2D &global_plan, unsigned int &x, unsigned int &y) |
![]() | |
void | propogateManhattanDistances () |
Go through the queue and set the cells to the Manhattan distance from their parents. More... | |
void | reset () override |
Clear the queue and set cell_values_ to the appropriate number of unreachableCellScore. More... | |
Additional Inherited Members | |
![]() | |
typedef std::shared_ptr< dwb_local_planner::TrajectoryCritic > | Ptr |
![]() | |
enum | ScoreAggregationType { ScoreAggregationType::Last, ScoreAggregationType::Sum, ScoreAggregationType::Product } |
Separate modes for aggregating scores across the multiple poses in a trajectory. More... | |
![]() | |
ScoreAggregationType | aggregationType_ |
nav_grid::VectorNavGrid< double > | cell_values_ |
double | obstacle_score_ |
std::shared_ptr< MapGridQueue > | queue_ |
bool | stop_on_failure_ |
double | unreachable_score_ |
Special cell_values. More... | |
![]() | |
nav_core2::Costmap::Ptr | costmap_ |
ros::NodeHandle | critic_nh_ |
std::string | name_ |
ros::NodeHandle | planner_nh_ |
double | scale_ |
Scores trajectories based on how far along the global path they end up.
This trajectory critic helps ensure progress along the global path. It finds the pose from the global path farthest from the robot that is still on the costmap, and aims for that point by assigning the lowest cost to the cell corresponding with that farthest pose.
Definition at line 50 of file goal_dist.h.
|
protected |
Definition at line 63 of file goal_dist.cpp.
|
overridevirtual |
Reimplemented from dwb_local_planner::TrajectoryCritic.
Definition at line 42 of file goal_dist.cpp.