Uses costmap 2d to assign negative costs if robot footprint is in obstacle on any point of the trajectory. More...

#include <nav2_dwb_controller/dwb_critics/include/dwb_critics/obstacle_footprint.hpp>

Inheritance diagram for dwb_critics::ObstacleFootprintCritic:

Collaboration diagram for dwb_critics::ObstacleFootprintCritic:

[legend]

Public Member Functions
bool	prepare (const geometry_msgs::msg::Pose2D &pose, const nav_2d_msgs::msg::Twist2D &vel, const geometry_msgs::msg::Pose2D &goal, const nav_2d_msgs::msg::Path2D &global_plan) override
	Prior to evaluating any trajectories, look at contextual information constant across all trajectories. More...

double	scorePose (const geometry_msgs::msg::Pose2D &pose) override
	Return the obstacle score for a particular pose. More...

virtual double	scorePose (const geometry_msgs::msg::Pose2D &pose, const Footprint &oriented_footprint)

double	getScale () const override

Public Member Functions inherited from dwb_critics::BaseObstacleCritic
void	onInit () override

double	scoreTrajectory (const dwb_msgs::msg::Trajectory2D &traj) override
	Return a raw score for the given trajectory. More...

void	addCriticVisualization (std::vector< std::pair< std::string, std::vector< float >>> &cost_channels) override
	Add information to the given pointcloud for debugging costmap-grid based scores. More...

virtual bool	isValidCost (const unsigned char cost)
	Check to see whether a given cell cost is valid for driving through. More...

Public Member Functions inherited from dwb_core::TrajectoryCritic
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	reset ()
	Reset the state of the critic. More...

virtual void	debrief (const nav_2d_msgs::msg::Twist2D &)
	debrief informs the critic what the chosen cmd_vel was (if it cares)

std::string	getName ()

void	setScale (const double scale)

Protected Member Functions
double	lineCost (int x0, int x1, int y0, int y1)
	Rasterizes a line in the costmap grid and checks for collisions. More...

double	pointCost (int x, int y)
	Checks the cost of a point in the costmap. More...

Protected Attributes
Footprint	footprint_spec_

Protected Attributes inherited from dwb_critics::BaseObstacleCritic
nav2_costmap_2d::Costmap2D *	costmap_

bool	sum_scores_

Protected Attributes inherited from dwb_core::TrajectoryCritic
std::string	name_

std::string	dwb_plugin_name_

std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap_ros_

double	scale_

rclcpp_lifecycle::LifecycleNode::WeakPtr	node_

Additional Inherited Members
Public Types inherited from dwb_core::TrajectoryCritic
using	Ptr = std::shared_ptr< dwb_core::TrajectoryCritic >

Detailed Description

Uses costmap 2d to assign negative costs if robot footprint is in obstacle on any point of the trajectory.

Internally, this technically only checks if the border of the footprint collides with anything for computational efficiency. This is valid if the obstacles in the local costmap are inflated.

A more robust class could check every cell within the robot's footprint without inflating the obstacles, at some computational cost. That is left as an excercise to the reader.

Definition at line 65 of file obstacle_footprint.hpp.

Member Function Documentation

◆ lineCost()

double dwb_critics::ObstacleFootprintCritic::lineCost	(	int	x0,
		int	x1,
		int	y0,
		int	y1
	)

protected

Rasterizes a line in the costmap grid and checks for collisions.

Parameters

x0	The x position of the first cell in grid coordinates
y0	The y position of the first cell in grid coordinates
x1	The x position of the second cell in grid coordinates
y1	The y position of the second cell in grid coordinates

Returns: A positive cost for a legal line... negative otherwise

Definition at line 134 of file obstacle_footprint.cpp.

References pointCost().

Referenced by nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker::footprintCost().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ pointCost()

double dwb_critics::ObstacleFootprintCritic::pointCost	(	int	x,
		int	y
	)

protected

Checks the cost of a point in the costmap.

Parameters

x	The x position of the point in cell coordinates
y	The y position of the point in cell coordinates

Returns: A positive cost for a legal point... negative otherwise

Definition at line 150 of file obstacle_footprint.cpp.

References nav2_costmap_2d::Costmap2D::getCost().

Referenced by lineCost(), and nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker::lineCost().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ prepare()

bool dwb_critics::ObstacleFootprintCritic::prepare	(	const geometry_msgs::msg::Pose2D &	,
		const nav_2d_msgs::msg::Twist2D &	,
		const geometry_msgs::msg::Pose2D &	,
		const nav_2d_msgs::msg::Path2D &
	)

overridevirtual

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 from dwb_core::TrajectoryCritic.

Definition at line 63 of file obstacle_footprint.cpp.