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 nav2::LifecycleNode::WeakPtr &parent, std::string name, nav2::TransformBuffer::SharedPtr 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 *, const nav_msgs::msg::Path &transformed_global_plan, const geometry_msgs::msg::PoseStamped &global_goal) 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	newPathReceived (const nav_msgs::msg::Path &raw_global_path) override
	nav2_core newPathReceived - Receives a new plan from the Planner Server 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 nav2::LifecycleNode::WeakPtr &, std::string name, nav2::TransformBuffer::SharedPtr, 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 (nav2_core::GoalChecker *goal_checker, const geometry_msgs::msg::PoseStamped &robot_pose, const geometry_msgs::msg::PoseStamped &goal_pose, const geometry_msgs::msg::Twist &speed, const nav_msgs::msg::Path &transformed_plan)
	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...

Protected Attributes
nav2::LifecycleNode::WeakPtr	node_

nav2::TransformBuffer::SharedPtr	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	control_duration_

bool	cancelling_ = false

bool	finished_cancelling_ = false

bool	is_rotating_to_heading_ = false

geometry_msgs::msg::Twist	last_command_velocity_

nav2::Publisher< geometry_msgs::msg::PointStamped >::SharedPtr	carrot_pub_

nav2::Publisher< geometry_msgs::msg::PointStamped >::SharedPtr	curvature_carrot_pub_

nav2::Publisher< std_msgs::msg::Bool >::SharedPtr	is_rotating_to_heading_pub_

nav2::Publisher< nav_msgs::msg::Path >::SharedPtr	carrot_arc_pub_

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 44 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 372 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 312 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,
		const nav_msgs::msg::Path &	transformed_global_plan,
		const geometry_msgs::msg::PoseStamped &	global_goal
	)

overridevirtual

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

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
transformed_global_plan	The global plan after being processed by the path handler
global_goal	The last pose of the global plan

Returns: Best command

Implements nav2_core::Controller.

Definition at line 161 of file regulated_pure_pursuit_controller.cpp.

◆ configure()

void nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::configure	(	const nav2::LifecycleNode::WeakPtr &	parent,
		std::string	name,
		nav2::TransformBuffer::SharedPtr	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 42 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 119 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 130 of file regulated_pure_pursuit_controller.cpp.

◆ newPathReceived()

void nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::newPathReceived ( const nav_msgs::msg::Path & raw_global_path )

overridevirtual

nav2_core newPathReceived - Receives a new plan from the Planner Server

Parameters

raw_global_path The global plan from the Planner Server

Implements nav2_core::Controller.

Definition at line 403 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 351 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 408 of file regulated_pure_pursuit_controller.cpp.

◆ shouldRotateToGoalHeading()

bool nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::shouldRotateToGoalHeading	(	nav2_core::GoalChecker *	goal_checker,
		const geometry_msgs::msg::PoseStamped &	robot_pose,
		const geometry_msgs::msg::PoseStamped &	goal_pose,
		const geometry_msgs::msg::Twist &	speed,
		const nav_msgs::msg::Path &	transformed_plan
	)

protected

Whether robot should rotate to final goal orientation.

Parameters

goal_checker	Goal checker instance for tolerances / state
robot_pose	Current robot pose in costmap's global frame
goal_pose	Goal pose in costmap's global frame
speed	Current robot speed
transformed_plan	The plan in the robot base frame

Returns: Whether should rotate to goal heading

Definition at line 336 of file regulated_pure_pursuit_controller.cpp.

References nav2_core::GoalChecker::isGoalXYReached().

Here is the call graph for this function:

◆ 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 322 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

Protected Attributes

Additional Inherited Members

Detailed Description

Member Function Documentation

◆ applyConstraints()

◆ cancel()

◆ computeVelocityCommands()

◆ configure()

◆ createCarrotMsg()

◆ getLookAheadDistance()

◆ newPathReceived()

◆ rotateToHeading()

◆ setSpeedLimit()

◆ shouldRotateToGoalHeading()

◆ shouldRotateToPath()