Regulated pure pursuit controller plugin. More...

#include <nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp>

Inheritance diagram for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController:

[legend]

Collaboration diagram for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController:

[legend]

Public Member Functions
	RegulatedPurePursuitController ()=default
	Constructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController.

	~RegulatedPurePursuitController () override=default
	Destrructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController.

void	configure (const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name, std::shared_ptr< tf2_ros::Buffer > tf, std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmap_ros) override
	Configure controller state machine. More...

void	cleanup () override
	Cleanup controller state machine.

void	activate () override
	Activate controller state machine.

void	deactivate () override
	Deactivate controller state machine.

geometry_msgs::msg::TwistStamped	computeVelocityCommands (const geometry_msgs::msg::PoseStamped &pose, const geometry_msgs::msg::Twist &velocity, nav2_core::GoalChecker *) override
	Compute the best command given the current pose and velocity, with possible debug information. More...

bool	cancel () override
	Cancel the current control action. More...

void	setPlan (const nav_msgs::msg::Path &path) override
	nav2_core setPlan - Sets the global plan More...

void	setSpeedLimit (const double &speed_limit, const bool &percentage) override
	Limits the maximum linear speed of the robot. More...

void	reset () override
	Reset the state of the controller if necessary after task is exited.

Public Member Functions inherited from nav2_core::Controller
virtual	~Controller ()
	Virtual destructor.

virtual void	configure (const rclcpp_lifecycle::LifecycleNode::WeakPtr &, std::string name, std::shared_ptr< tf2_ros::Buffer >, std::shared_ptr< nav2_costmap_2d::Costmap2DROS >)=0

Protected Member Functions
double	getLookAheadDistance (const geometry_msgs::msg::Twist &)
	Get lookahead distance. More...

std::unique_ptr< geometry_msgs::msg::PointStamped >	createCarrotMsg (const geometry_msgs::msg::PoseStamped &carrot_pose)
	Creates a PointStamped message for visualization. More...

bool	shouldRotateToPath (const geometry_msgs::msg::PoseStamped &carrot_pose, double &angle_to_path, double &x_vel_sign)
	Whether robot should rotate to rough path heading. More...

bool	shouldRotateToGoalHeading (const geometry_msgs::msg::PoseStamped &carrot_pose)
	Whether robot should rotate to final goal orientation. More...

void	rotateToHeading (double &linear_vel, double &angular_vel, const double &angle_to_path, const geometry_msgs::msg::Twist &curr_speed)
	Create a smooth and kinematically smoothed rotation command. More...

void	applyConstraints (const double &curvature, const geometry_msgs::msg::Twist &speed, const double &pose_cost, const nav_msgs::msg::Path &path, double &linear_vel, double &sign)
	apply regulation constraints to the system More...

geometry_msgs::msg::PoseStamped	getLookAheadPoint (const double &, const nav_msgs::msg::Path &, bool interpolate_after_goal=false)
	Get lookahead point. More...

double	findVelocitySignChange (const nav_msgs::msg::Path &transformed_plan)
	checks for the cusp position More...

Static Protected Member Functions
static geometry_msgs::msg::Point	circleSegmentIntersection (const geometry_msgs::msg::Point &p1, const geometry_msgs::msg::Point &p2, double r)
	Find the intersection a circle and a line segment. This assumes the circle is centered at the origin. If no intersection is found, a floating point error will occur. More...

Protected Attributes
rclcpp_lifecycle::LifecycleNode::WeakPtr	node_

std::shared_ptr< tf2_ros::Buffer >	tf_

std::string	plugin_name_

std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap_ros_

nav2_costmap_2d::Costmap2D *	costmap_

rclcpp::Logger	logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}

Parameters *	params_

double	goal_dist_tol_

double	control_duration_

bool	cancelling_ = false

bool	finished_cancelling_ = false

bool	is_rotating_to_heading_ = false

bool	has_reached_xy_tolerance_ = false

std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< nav_msgs::msg::Path > >	global_path_pub_

std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< geometry_msgs::msg::PointStamped > >	carrot_pub_

std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< geometry_msgs::msg::PointStamped > >	curvature_carrot_pub_

std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< std_msgs::msg::Bool > >	is_rotating_to_heading_pub_

std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< nav_msgs::msg::Path > >	carrot_arc_pub_

std::unique_ptr< nav2_regulated_pure_pursuit_controller::PathHandler >	path_handler_

std::unique_ptr< nav2_regulated_pure_pursuit_controller::ParameterHandler >	param_handler_

std::unique_ptr< nav2_regulated_pure_pursuit_controller::CollisionChecker >	collision_checker_

Additional Inherited Members
Public Types inherited from nav2_core::Controller
using	Ptr = std::shared_ptr< nav2_core::Controller >

Detailed Description

Regulated pure pursuit controller plugin.

Definition at line 43 of file regulated_pure_pursuit_controller.hpp.

Member Function Documentation

◆ applyConstraints()

void nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::applyConstraints	(	const double &	curvature,
		const geometry_msgs::msg::Twist &	speed,
		const double &	pose_cost,
		const nav_msgs::msg::Path &	path,
		double &	linear_vel,
		double &	sign
	)

protected

apply regulation constraints to the system

Parameters

linear_vel	robot command linear velocity input
lookahead_dist	optimal lookahead distance
curvature	curvature of path
speed	Speed of robot
pose_cost	cost at this pose

Definition at line 449 of file regulated_pure_pursuit_controller.cpp.

◆ cancel()

bool nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::cancel ( )

overridevirtual

Cancel the current control action.

Returns: True if the cancellation was successful. If false is returned, computeVelocityCommands will be called until cancel returns true.

Reimplemented from nav2_core::Controller.

Definition at line 295 of file regulated_pure_pursuit_controller.cpp.

◆ circleSegmentIntersection()

geometry_msgs::msg::Point nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::circleSegmentIntersection	(	const geometry_msgs::msg::Point &	p1,
		const geometry_msgs::msg::Point &	p2,
		double	r
	)

staticprotected

Find the intersection a circle and a line segment. This assumes the circle is centered at the origin. If no intersection is found, a floating point error will occur.

Parameters

p1	first endpoint of line segment
p2	second endpoint of line segment
r	radius of circle

Returns: point of intersection

Definition at line 355 of file regulated_pure_pursuit_controller.cpp.

◆ computeVelocityCommands()

geometry_msgs::msg::TwistStamped nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::computeVelocityCommands	(	const geometry_msgs::msg::PoseStamped &	pose,
		const geometry_msgs::msg::Twist &	velocity,
		nav2_core::GoalChecker *	goal_checker
	)

overridevirtual

Compute the best command given the current pose and velocity, with possible debug information.

Same as above computeVelocityCommands, but with debug results. If the results pointer is not null, additional information about the twists evaluated will be in results after the call.

Parameters

pose	Current robot pose
velocity	Current robot velocity
goal_checker	Ptr to the goal checker for this task in case useful in computing commands

Returns: Best command

Implements nav2_core::Controller.

Definition at line 164 of file regulated_pure_pursuit_controller.cpp.

References nav2_core::GoalChecker::getTolerances().

Here is the call graph for this function:

◆ configure()

void nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::configure	(	const rclcpp_lifecycle::LifecycleNode::WeakPtr &	parent,
		std::string	name,
		std::shared_ptr< tf2_ros::Buffer >	tf,
		std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap_ros
	)

override

Configure controller state machine.

Parameters

parent	WeakPtr to node
name	Name of plugin
tf	TF buffer
costmap_ros	Costmap2DROS object of environment

Definition at line 39 of file regulated_pure_pursuit_controller.cpp.

◆ createCarrotMsg()

std::unique_ptr< geometry_msgs::msg::PointStamped > nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::createCarrotMsg ( const geometry_msgs::msg::PoseStamped & carrot_pose )

protected

Creates a PointStamped message for visualization.

Parameters

carrot_pose Input carrot point as a PoseStamped

Returns: CarrotMsg a carrot point marker, PointStamped

Definition at line 122 of file regulated_pure_pursuit_controller.cpp.

◆ findVelocitySignChange()

double nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::findVelocitySignChange ( const nav_msgs::msg::Path & transformed_plan )

protected

checks for the cusp position

Parameters

pose	Pose input to determine the cusp position

Returns: robot distance from the cusp

Definition at line 513 of file regulated_pure_pursuit_controller.cpp.

◆ getLookAheadDistance()

double nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::getLookAheadDistance ( const geometry_msgs::msg::Twist & speed )

protected

Get lookahead distance.

Parameters

cmd	the current speed to use to compute lookahead point

Returns: lookahead distance

Definition at line 133 of file regulated_pure_pursuit_controller.cpp.

◆ getLookAheadPoint()

geometry_msgs::msg::PoseStamped nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::getLookAheadPoint	(	const double &	lookahead_dist,
		const nav_msgs::msg::Path &	transformed_plan,
		bool	interpolate_after_goal = `false`
	)

protected

Get lookahead point.

Parameters

lookahead_dist	Optimal lookahead distance
path	Current global path
interpolate_after_goal	If true, interpolate the lookahead point after the goal based on the orientation given by the position of the last two pose of the path

Returns: Lookahead point

Definition at line 390 of file regulated_pure_pursuit_controller.cpp.

◆ rotateToHeading()

void nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::rotateToHeading	(	double &	linear_vel,
		double &	angular_vel,
		const double &	angle_to_path,
		const geometry_msgs::msg::Twist &	curr_speed
	)

protected

Create a smooth and kinematically smoothed rotation command.

Parameters

linear_vel	linear velocity
angular_vel	angular velocity
angle_to_path	Angle of robot output relative to carrot marker
curr_speed	the current robot speed

Definition at line 340 of file regulated_pure_pursuit_controller.cpp.

◆ setPlan()

void nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::setPlan ( const nav_msgs::msg::Path & path )

overridevirtual

nav2_core setPlan - Sets the global plan

Parameters

path	The global plan

Implements nav2_core::Controller.

Definition at line 480 of file regulated_pure_pursuit_controller.cpp.

◆ setSpeedLimit()

void nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::setSpeedLimit	(	const double &	speed_limit,
		const bool &	percentage
	)

overridevirtual

Limits the maximum linear speed of the robot.

Parameters

speed_limit	expressed in absolute value (in m/s) or in percentage from maximum robot speed.
percentage	Setting speed limit in percentage if true or in absolute values in false case.

Implements nav2_core::Controller.

Definition at line 486 of file regulated_pure_pursuit_controller.cpp.

◆ shouldRotateToGoalHeading()

bool nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::shouldRotateToGoalHeading ( const geometry_msgs::msg::PoseStamped & carrot_pose )

protected

Whether robot should rotate to final goal orientation.

Parameters

carrot_pose current lookahead point

Returns: Whether should rotate to goal heading

Definition at line 319 of file regulated_pure_pursuit_controller.cpp.

◆ shouldRotateToPath()

bool nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::shouldRotateToPath	(	const geometry_msgs::msg::PoseStamped &	carrot_pose,
		double &	angle_to_path,
		double &	x_vel_sign
	)

protected

Whether robot should rotate to rough path heading.

Parameters

carrot_pose	current lookahead point
angle_to_path	Angle of robot output relative to carrot marker
x_vel_sign	Velocoty sign (forward or backward)

Returns: Whether should rotate to path heading

Definition at line 305 of file regulated_pure_pursuit_controller.cpp.

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

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

Public Member Functions

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Function Documentation

◆ applyConstraints()

◆ cancel()

◆ circleSegmentIntersection()

◆ computeVelocityCommands()

◆ configure()

◆ createCarrotMsg()

◆ findVelocitySignChange()

◆ getLookAheadDistance()

◆ getLookAheadPoint()

◆ rotateToHeading()

◆ setPlan()

◆ setSpeedLimit()

◆ shouldRotateToGoalHeading()

◆ shouldRotateToPath()