Class VectorPursuitController

• Defined in File vector_pursuit_controller.hpp

Inheritance Relationships

Base Type

• public nav2_core::Controller

Class Documentation

class VectorPursuitController : public nav2_core::Controller

Public Functions

VectorPursuitController() = default

Constructor for nav2_vector_pursuit::VectorPursuitController.

~VectorPursuitController() override = default

Destrructor for nav2_vector_pursuit::VectorPursuitController.

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

geometry_msgs::msg::Quaternion getOrientation(const geometry_msgs::msg::Point &p1, const geometry_msgs::msg::Point &p2)

Get the orientation of the line segment between two points.

Parameters:

• p1 – The first point

• p2 – The second point

Returns:

The angle between the two points

double calcTurningRadius(const geometry_msgs::msg::PoseStamped &target_pose)

Calculate the turning radius of the robot given a target point.

Parameters:

target_pose – The target pose to calculate the turning radius

Returns:

The turning radius of the robot

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

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

bool isCollisionImminent(const geometry_msgs::msg::PoseStamped &robot_pose, const double &linear_vel, const double &angular_vel, const double &target_dist)

Whether collision is imminent.

Parameters:

• robot_pose – Pose of robot

• linear_vel – linear velocity to forward project

• angular_vel – angular velocity to forward project

• target_dist – Distance to the lookahead point

Returns:

Whether collision is imminent

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

Get lookahead point.

The lookahead point could:

1. Be discrete/interpolated

2. Have a heading: a. Directly from path (discrete) b. Directly from path (interpolated) (if use_interpolation_ is true) c. Computed from path (if use_heading_from_path_ is false)

Hence the flags use_interpolation_ and use_heading_from_path_ play important roles here.

Parameters:

• lookahead_dist – Optimal lookahead distance

• path – Current global path

Returns:

Lookahead point

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

Apply approach velocity scaling to the system.

Parameters:

• path – Transformed global path

• linear_vel – robot command linear velocity input

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

Apply cost and curvature constraints to the system.

Parameters:

• curvature – curvature of path

• curr_speed – Speed of robot

• pose_cost – cost at this pose

• linear_vel – robot command linear velocity input

• path – Transformed global path

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

Whether robot should rotate to rough path heading.

Parameters:

• target_pose – current lookahead point

• angle_to_path – Angle of robot output relative to lookahead point

Returns:

Whether should rotate to path heading

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

Whether robot should rotate to final goal orientation.

Parameters:

target_pose – current lookahead point

Returns:

Whether should rotate to goal heading

double getCuspDist(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

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 relative to lookahead point

• curr_speed – the current robot speed

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_buffer_

std::string plugin_name_

std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_

nav2_costmap_2d::Costmap2D *costmap_

rclcpp::Logger logger_ = {rclcpp::get_logger("VectorPursuitController")}

rclcpp::Clock::SharedPtr clock_

double k_

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 min_turning_radius_

double min_linear_velocity_

double max_lateral_accel_

double control_duration_

double max_allowed_time_to_collision_up_to_target_

bool use_collision_detection_

bool use_cost_regulated_linear_velocity_scaling_

double cost_scaling_dist_

double cost_scaling_gain_

double inflation_cost_scaling_factor_

bool use_rotate_to_heading_

double max_angular_accel_

double max_linear_accel_

double rotate_to_heading_min_angle_

double goal_dist_tol_

double max_robot_pose_search_dist_

bool use_interpolation_

bool allow_reversing_

bool is_reversing_

bool use_heading_from_path_

geometry_msgs::msg::Twist last_cmd_vel_

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::PoseStamped>> target_pub_

std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> target_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