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

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:

  • Outside the local_costmap (collision avoidance cannot be assured)

Parameters:

pose – pose to transform

Returns:

Path in new frame

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.

Parameters:

  • frame – Frame ID to transform to

  • in_pose – Pose input to transform

  • out_pose – transformed output

Returns:

bool if successful

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

bool isCollisionImminent(const geometry_msgs::msg::PoseStamped&, const double&, const double&, const double&)

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

bool inCollision(const double &x, const double &y, const double &theta)

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

double costAtPose(const double &x, const double &y)

Cost at a point.

Parameters:

  • x – Pose of pose x

  • y – Pose of pose y

Returns:

Cost of pose in costmap

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

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

double getCostmapMaxExtent() const

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

Returns:

max of distance from center in meters to edge of costmap

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

Callback executed when a parameter change is detected.

Parameters:

event – ParameterEvent message

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_

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