Class BaseObstacleCritic

Inheritance Relationships

Base Type

  • public dwb_core::TrajectoryCritic

Derived Type

Class Documentation

class BaseObstacleCritic : public dwb_core::TrajectoryCritic

Uses costmap 2d to assign negative costs if a circular robot would collide at any point of the trajectory.

This class can only be used to figure out if a circular robot is in collision. If the cell corresponding with any of the poses in the Trajectory is an obstacle, inscribed obstacle or unknown, it will return a negative cost. Otherwise it will return either the final pose’s cost, or the sum of all poses, depending on the sum_scores parameter.

Other classes (like ObstacleFootprintCritic) can do more advanced checking for collisions.

Subclassed by dwb_critics::ObstacleFootprintCritic

Public Functions

void onInit() override
virtual double scoreTrajectory(const dwb_msgs::msg::Trajectory2D &traj) override
virtual void addCriticVisualization(std::vector<std::pair<std::string, std::vector<float>>> &cost_channels) override
virtual double scorePose(const geometry_msgs::msg::Pose2D &pose)

Return the obstacle score for a particular pose.

Parameters:

pose – Pose to check

virtual bool isValidCost(const unsigned char cost)

Check to see whether a given cell cost is valid for driving through.

Parameters:

cost – Cost of the cell

Returns:

Return true if valid cell

Protected Attributes

nav2_costmap_2d::Costmap2D *costmap_
bool sum_scores_