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

#include <rotate_to_goal.h>

Inheritance diagram for dwb_critics::RotateToGoalCritic:

Public Member Functions
void	onInit () override

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	reset () override

virtual double	scoreRotation (const dwb_msgs::Trajectory2D &traj)
	Assuming that this is an actual rotation when near the goal, score the trajectory. More...

double	scoreTrajectory (const dwb_msgs::Trajectory2D &traj) override

Public Member Functions inherited from dwb_local_planner::TrajectoryCritic
virtual void	addCriticVisualization (sensor_msgs::PointCloud &pc)

virtual void	debrief (const nav_2d_msgs::Twist2D &cmd_vel)

std::string	getName ()

virtual double	getScale () const

void	initialize (const ros::NodeHandle &planner_nh, std::string name, nav_core2::Costmap::Ptr costmap)

void	setScale (const double scale)

virtual	~TrajectoryCritic ()

Protected Attributes
double	current_xy_speed_sq_

double	goal_yaw_

bool	in_window_

double	lookahead_time_

bool	rotating_

double	slowing_factor_

double	stopped_xy_velocity_sq_

double	xy_goal_tolerance_

double	xy_goal_tolerance_sq_
	Cached squared tolerance. More...

Protected Attributes inherited from dwb_local_planner::TrajectoryCritic
nav_core2::Costmap::Ptr	costmap_

ros::NodeHandle	critic_nh_

std::string	name_

ros::NodeHandle	planner_nh_

double	scale_

Additional Inherited Members
Public Types inherited from dwb_local_planner::TrajectoryCritic
typedef std::shared_ptr< dwb_local_planner::TrajectoryCritic >	Ptr

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.h.

Member Function Documentation

void dwb_critics::RotateToGoalCritic::onInit ( )

overridevirtual

Reimplemented from dwb_local_planner::TrajectoryCritic.

Definition at line 55 of file rotate_to_goal.cpp.

bool dwb_critics::RotateToGoalCritic::prepare	(	const geometry_msgs::Pose2D &	pose,
		const nav_2d_msgs::Twist2D &	vel,
		const geometry_msgs::Pose2D &	goal,
		const nav_2d_msgs::Path2D &	global_plan
	)

overridevirtual

Reimplemented from dwb_local_planner::TrajectoryCritic.

Definition at line 72 of file rotate_to_goal.cpp.

void dwb_critics::RotateToGoalCritic::reset ( )

overridevirtual

Reimplemented from dwb_local_planner::TrajectoryCritic.

Definition at line 66 of file rotate_to_goal.cpp.

double dwb_critics::RotateToGoalCritic::scoreRotation ( const dwb_msgs::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

traj	Trajectory to score

Returns: numeric score

Definition at line 110 of file rotate_to_goal.cpp.

double dwb_critics::RotateToGoalCritic::scoreTrajectory ( const dwb_msgs::Trajectory2D & traj )

overridevirtual

Implements dwb_local_planner::TrajectoryCritic.

Definition at line 84 of file rotate_to_goal.cpp.

Member Data Documentation

double dwb_critics::RotateToGoalCritic::current_xy_speed_sq_

protected

Definition at line 94 of file rotate_to_goal.h.

double dwb_critics::RotateToGoalCritic::goal_yaw_

protected

Definition at line 91 of file rotate_to_goal.h.

bool dwb_critics::RotateToGoalCritic::in_window_

protected

Definition at line 90 of file rotate_to_goal.h.

double dwb_critics::RotateToGoalCritic::lookahead_time_

protected

Definition at line 96 of file rotate_to_goal.h.

bool dwb_critics::RotateToGoalCritic::rotating_

protected

Definition at line 90 of file rotate_to_goal.h.

double dwb_critics::RotateToGoalCritic::slowing_factor_

protected

Definition at line 95 of file rotate_to_goal.h.

double dwb_critics::RotateToGoalCritic::stopped_xy_velocity_sq_

protected

Definition at line 94 of file rotate_to_goal.h.

double dwb_critics::RotateToGoalCritic::xy_goal_tolerance_

protected

Definition at line 92 of file rotate_to_goal.h.

double dwb_critics::RotateToGoalCritic::xy_goal_tolerance_sq_

protected

Cached squared tolerance.

Definition at line 93 of file rotate_to_goal.h.

The documentation for this class was generated from the following files:

Public Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Function Documentation

Member Data Documentation