Scores trajectories based on how far from the global path the front of the robot ends up. More...

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

Inheritance diagram for dwb_critics::PathAlignCritic:

[legend]

Collaboration diagram for dwb_critics::PathAlignCritic:

[legend]

Public Member Functions
void	onInit () override

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	getScale () const override

double	scorePose (const geometry_msgs::msg::Pose2D &pose) override
	Retrieve the score for a single pose. More...

Public Member Functions inherited from dwb_critics::PathDistCritic
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...

Public Member Functions inherited from dwb_critics::MapGridCritic
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...

double	getScore (unsigned int x, unsigned int y)
	Retrieve the score for a particular cell of the costmap. More...

void	setAsObstacle (unsigned int index)
	Sets the score of a particular cell to the obstacle cost. 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	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 Attributes
bool	zero_scale_

double	forward_point_distance_

Protected Attributes inherited from dwb_critics::MapGridCritic
std::shared_ptr< MapGridQueue >	queue_

nav2_costmap_2d::Costmap2D *	costmap_

std::vector< double >	cell_values_

double	obstacle_score_

double	unreachable_score_
	Special cell_values.

bool	stop_on_failure_

ScoreAggregationType	aggregationType_

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 >

Protected Types inherited from dwb_critics::MapGridCritic
enum class	ScoreAggregationType { Last , Sum , Product }
	Separate modes for aggregating scores across the multiple poses in a trajectory. More...

Protected Member Functions inherited from dwb_critics::MapGridCritic
void	reset () override
	Clear the queuDWB_CRITICS_MAP_GRID_He and set cell_values_ to the appropriate number of unreachableCellScore.

void	propogateManhattanDistances ()
	Go through the queue and set the cells to the Manhattan distance from their parents.

Detailed Description

Scores trajectories based on how far from the global path the front of the robot ends up.

This uses the costmap grid as a proxy for calculating which way the robot should be facing relative to the global path. Instead of scoring how far the center of the robot is away from the global path, this critic calculates how far a point forward_point_distance in front of the robot is from the global path. This biases the planner toward trajectories that line up with the global plan.

When the robot is near the end of the path, the scale of this critic is set to zero. When the projected point is past the global goal, we no longer want this critic to try to align to a part of the global path that isn't there.

Definition at line 56 of file path_align.hpp.

Member Function Documentation

◆ prepare()

bool dwb_critics::PathAlignCritic::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 60 of file path_align.cpp.

References dwb_critics::PathDistCritic::prepare().

Here is the call graph for this function: