Forces the commanded trajectories to only be rotations if within a certain distance window. More...

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

Inheritance diagram for dwb_critics::RotateToGoalCritic:

Collaboration diagram for dwb_critics::RotateToGoalCritic:

Public Member Functions
void	onInit () override

void	reset () override
	Reset the state of the critic. More...

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	scoreTrajectory (const dwb_msgs::msg::Trajectory2D &traj) override
	Return a raw score for the given trajectory. More...

virtual double	scoreRotation (const dwb_msgs::msg::Trajectory2D &traj)
	Assuming that this is an actual rotation when near the goal, score the trajectory. 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)

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

std::string	getName ()

virtual double	getScale () const

void	setScale (const double scale)

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

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_

Detailed Description

Forces the commanded trajectories to only be rotations if within a certain distance window.

This used to be built in to the DWA Local Planner as the LatchedStopRotate controller, but has been moved to a critic for consistency.

The critic has three distinct phases. 1) If the current pose is outside xy_goal_tolerance LINEAR distance from the goal pose, this critic will just return score 0.0. 2) If within the xy_goal_tolerance and the robot is still moving with non-zero linear motion, this critic will only allow trajectories that are slower than the current speed in order to stop the robot (within the robot's acceleration limits). The returned score will be the robot's linear speed squared, multiplied by the slowing_factor parameter (default 5.0) added to the result of scoreRotation. 3) If within the xy_goal_tolerance and the robot has sufficiently small linear motion, this critic will score trajectories that have linear movement as invalid and score the rest based on the result of the scoreRotation method

The scoreRotation method can be overriden, but the default behavior is to return the shortest angular distance between the goal pose and a pose from the trajectory. Which pose depends on the lookahead_time parameter.

If the lookahead_time parameter is negative, the pose evaluated will be the last pose in the trajectory, which is the same as DWA's behavior. This is the default.
Otherwise, a new pose will be projected using the dwb_local_planner::projectPose. By using a lookahead time shorter than sim_time, the critic will be less concerned about overshooting the goal yaw and thus will continue to turn faster for longer.

Definition at line 70 of file rotate_to_goal.hpp.

Member Function Documentation

◆ prepare()

bool dwb_critics::RotateToGoalCritic::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 84 of file rotate_to_goal.cpp.

◆ reset()

void dwb_critics::RotateToGoalCritic::reset ( )

overridevirtual

Reset the state of the critic.

Reset is called when the planner receives a new global plan. This can be used to discard information specific to one plan.

Reimplemented from dwb_core::TrajectoryCritic.

Definition at line 78 of file rotate_to_goal.cpp.

◆ scoreRotation()

double dwb_critics::RotateToGoalCritic::scoreRotation ( const dwb_msgs::msg::Trajectory2D & traj )

virtual

Assuming that this is an actual rotation when near the goal, score the trajectory.

This (easily overridden) method assumes that the critic is in the third phase (as described above) and returns a numeric score for the trajectory relative to the goal yaw.

Parameters