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

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

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

Protected Member Functions
nav_msgs::msg::Path	transformGlobalPlan (const geometry_msgs::msg::PoseStamped &pose)
	Transforms global plan into same frame as pose and clips poses ineligible for lookaheadPoint Points ineligible to be selected as a lookahead point if they are any of the following: More...

bool	transformPose (const std::string frame, const geometry_msgs::msg::PoseStamped &in_pose, geometry_msgs::msg::PoseStamped &out_pose) const
	Transform a pose to another frame. More...

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

bool	isCollisionImminent (const geometry_msgs::msg::PoseStamped &, const double &, const double &, const double &)
	Whether collision is imminent. More...

bool	inCollision (const double &x, const double &y, const double &theta)
	checks for collision at projected pose More...

double	costAtPose (const double &x, const double &y)
	Cost at a point. More...

double	approachVelocityScalingFactor (const nav_msgs::msg::Path &path) const

void	applyApproachVelocityScaling (const nav_msgs::msg::Path &path, double &linear_vel) const

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 &)
	Get lookahead point. More...

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

double	getCostmapMaxExtent () const

rcl_interfaces::msg::SetParametersResult	dynamicParametersCallback (std::vector< rclcpp::Parameter > parameters)
	Callback executed when a parameter change is detected. 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")}

rclcpp::Clock::SharedPtr	clock_

double	desired_linear_vel_

double	base_desired_linear_vel_

double	lookahead_dist_

double	rotate_to_heading_angular_vel_

double	max_lookahead_dist_

double	min_lookahead_dist_

double	lookahead_time_

bool	use_velocity_scaled_lookahead_dist_

tf2::Duration	transform_tolerance_

double	min_approach_linear_velocity_

double	approach_velocity_scaling_dist_

double	control_duration_

double	max_allowed_time_to_collision_up_to_carrot_

bool	use_collision_detection_

bool	use_regulated_linear_velocity_scaling_

bool	use_cost_regulated_linear_velocity_scaling_

double	cost_scaling_dist_

double	cost_scaling_gain_

double	inflation_cost_scaling_factor_

double	regulated_linear_scaling_min_radius_

double	regulated_linear_scaling_min_speed_

bool	use_rotate_to_heading_

double	max_angular_accel_

double	rotate_to_heading_min_angle_

double	goal_dist_tol_

bool	allow_reversing_

double	max_robot_pose_search_dist_

bool	use_interpolation_

nav_msgs::msg::Path	global_plan_

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< nav_msgs::msg::Path > >	carrot_arc_pub_

std::unique_ptr< nav2_costmap_2d::FootprintCollisionChecker< nav2_costmap_2d::Costmap2D * > >	collision_checker_

std::mutex	mutex_

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr	dyn_params_handler_

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 41 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 630 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 401 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 278 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
	)

overridevirtual

Configure controller state machine.

Parameters

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

Possible to drive in reverse direction if and only if "use_rotate_to_heading" parameter is set to false

Implements nav2_core::Controller.

Definition at line 41 of file regulated_pure_pursuit_controller.cpp.

◆ costAtPose()

double nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::costAtPose	(	const double &	x,
		const double &	y
	)

protected

Cost at a point.

Parameters

x	Pose of pose x
y	Pose of pose y

Returns: Cost of pose in costmap

Definition at line 576 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 253 of file regulated_pure_pursuit_controller.cpp.

◆ dynamicParametersCallback()

rcl_interfaces::msg::SetParametersResult nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::dynamicParametersCallback ( std::vector< rclcpp::Parameter > parameters )

protected

Callback executed when a parameter change is detected.

Parameters

event ParameterEvent message

Definition at line 820 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 761 of file regulated_pure_pursuit_controller.cpp.

◆ getCostmapMaxExtent()

double nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::getCostmapMaxExtent ( ) const

protected

Get the greatest extent of the costmap in meters from the center.

Returns: max of distance from center in meters to edge of costmap

Definition at line 811 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 264 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
	)

protected

Get lookahead point.

Parameters

lookahead_dist	Optimal lookahead distance
path	Current global path

Returns: Lookahead point

Definition at line 436 of file regulated_pure_pursuit_controller.cpp.

◆ inCollision()

bool nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::inCollision	(	const double &	x,
		const double &	y,
		const double &	theta
	)

protected

checks for collision at projected pose

Parameters

x	Pose of pose x
y	Pose of pose y
theta	orientation of Yaw

Returns: Whether in collision

Definition at line 548 of file regulated_pure_pursuit_controller.cpp.

◆ isCollisionImminent()

bool nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::isCollisionImminent	(	const geometry_msgs::msg::PoseStamped &	robot_pose,
		const double &	linear_vel,
		const double &	angular_vel,
		const double &	carrot_dist
	)

protected

Whether collision is imminent.

Parameters

robot_pose	Pose of robot
carrot_pose	Pose of carrot
linear_vel	linear velocity to forward project
angular_vel	angular velocity to forward project
carrot_dist	Distance to the carrot for PP

Returns: Whether collision is imminent

Definition at line 469 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 relatie to carrot marker
curr_speed	the current robot speed

Definition at line 386 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 669 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 674 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 378 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
	)

protected

Whether robot should rotate to rough path heading.

Parameters

carrot_pose	current lookahead point
angle_to_path	Angle of robot output relatie to carrot marker

Returns: Whether should rotate to path heading

Definition at line 370 of file regulated_pure_pursuit_controller.cpp.

◆ transformGlobalPlan()

nav_msgs::msg::Path nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::transformGlobalPlan ( const geometry_msgs::msg::PoseStamped & pose )

protected

Transforms global plan into same frame as pose and clips poses ineligible for lookaheadPoint Points ineligible to be selected as a lookahead point if they are any of the following:

Outside the local_costmap (collision avoidance cannot be assured)
Parameters

pose pose to transform

Returns
Path in new frame

Definition at line 692 of file regulated_pure_pursuit_controller.cpp.

◆ transformPose()

bool nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController::transformPose	(	const std::string	frame,
		const geometry_msgs::msg::PoseStamped &	in_pose,
		geometry_msgs::msg::PoseStamped &	out_pose
	)		const

protected

Transform a pose to another frame.

Parameters

frame	Frame ID to transform to
in_pose	Pose input to transform
out_pose	transformed output

Returns: bool if successful

Definition at line 791 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()

◆ circleSegmentIntersection()

◆ computeVelocityCommands()

◆ configure()

◆ costAtPose()

◆ createCarrotMsg()

◆ dynamicParametersCallback()

◆ findVelocitySignChange()

◆ getCostmapMaxExtent()

◆ getLookAheadDistance()

◆ getLookAheadPoint()

◆ inCollision()

◆ isCollisionImminent()

◆ rotateToHeading()

◆ setPlan()

◆ setSpeedLimit()

◆ shouldRotateToGoalHeading()

◆ shouldRotateToPath()

◆ transformGlobalPlan()

◆ transformPose()