Class RegulatedPurePursuitController

Inheritance Relationships

Base Type

  • public nav2_core::Controller

Class Documentation

class RegulatedPurePursuitController : public nav2_core::Controller

Regulated pure pursuit controller plugin.

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

Parameters:

  • parent – WeakPtr to node

  • name – Name of plugin

  • tf – TF buffer

  • costmap_ros – Costmap2DROS object of environment

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.

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

void setPlan(const nav_msgs::msg::Path &path) override

nav2_core setPlan - Sets the global plan

Parameters:

path – The global plan

void setSpeedLimit(const double &speed_limit, const bool &percentage) override

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.

Protected Functions

double getLookAheadDistance(const geometry_msgs::msg::Twist&)

Get lookahead distance.

Parameters:

cmd – the current speed to use to compute lookahead point

Returns:

lookahead distance

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

Creates a PointStamped message for visualization.

Parameters:

carrot_pose – Input carrot point as a PoseStamped

Returns:

CarrotMsg a carrot point marker, PointStamped

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

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

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

Whether robot should rotate to final goal orientation.

Parameters:

carrot_pose – current lookahead point

Returns:

Whether should rotate to goal heading

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.

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

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

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

geometry_msgs::msg::PoseStamped getLookAheadPoint(const double&, const nav_msgs::msg::Path&)

Get lookahead point.

Parameters:

  • lookahead_dist – Optimal lookahead distance

  • path – Current global path

Returns:

Lookahead point

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

checks for the cusp position

Parameters:

pose – Pose input to determine the cusp position

Returns:

robot distance from the cusp

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

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

Parameters:

  • p1 – first endpoint of line segment

  • p2 – second endpoint of line segment

  • r – radius of circle

Returns:

point of intersection