pure_pursuit_planner: A C++ repository from Arcanain

Functional Overview

This software implements a ROS 2 node for path following control of robots or autonomous vehicles using the Pure Pursuit algorithm. It calculates and outputs velocity and angular velocity commands to efficiently navigate the robot along a specified path.

Requirements

System Requirements

OS : Ubuntu 22.04
ROS2 : Humble

System Dependencies

How To Use

Execution Steps

cd ~/ros2_ws
source ~/ros2_ws/install/setup.bash
ros2 launch pure_pursuit_planner pure_pursuit_planner.py

Folder Structure

pure_pursuit_planner/
├── include/                               - Directory for header files
│   └── pure_pursuit_planner/              - Header files for the pure_pursuit_planner package
│       └── pure_pursuit_planner_component.hpp - Header file for the Pure Pursuit planner component
├── launch/                                - Directory for ROS 2 launch files
│   └── pure_pursuit_planner.py            - Launch script for the pure_pursuit_planner node
├── rviz/                                  - Directory for RViz configuration files
│   └── pure_pursuit_planner.rviz          - RViz configuration file for the Pure Pursuit planner
├── src/                                   - Directory for source files
│   ├── pure_pursuit_planner_component.cpp - Implementation of the Pure Pursuit planner component
│   └── pure_pursuit_planner_node.cpp      - Node implementation for the Pure Pursuit planner
├── CMakeLists.txt                         - CMake build configuration file
└── package.xml                            - ROS 2 package metadata file

Interface Table

Input

Variable Name	Type	Description
`odom`	`nav_msgs::msg::Odometry`	Odometry information of the robot
`tgt_path`	`nav_msgs::msg::Path`	Target trajectory of the robot

Output

Variable Name	Type	Description
`cmd_vel`	`geometry_msgs::msg::Twist`	Velocity and angular velocity commands for the robot

Internal Values

Variable Name	Type	Description
`x`, `y`, `yaw`	`double`	Current position and orientation of the robot
`v`, `w`	`double`	Velocity and angular velocity of the robot
`cx`, `cy`,`cyaw`, `ck`	`std::vector<double>`	List of x and y coordinates of the path
`target_ind`	`int`	Current target index
`target_vel`	`double`	Target velocity
`goal_threshold`	`double`	Threshold for goal judgment
`k`, `Lfc`, `Kp`, `dt`	`double`	Pure Pursuit parameters
`oldNearestPointIndex`	`int`	Index of the nearest point in the previous iteration
`current_vel`	`double`	Current velocity of the robot
`minCurvature`,`maxCurvature`	`double`	Minimum and maximum curvature values
`minVelocity`,`maxVelocity`	`double`	Minimum and maximum velocity values

Software architecture

Class Diagram

classDiagram
    class PurePursuitNode {
        +PurePursuitNode()
        -void updateControl()
        -std::pair<double, double> purePursuitControl(int&)
        -std::pair<int, double> searchTargetIndex()
        -double calcDistance(double, double) const
        -void odometry_callback(nav_msgs::msg::Odometry::SharedPtr)
        -void path_callback(nav_msgs::msg::Path::SharedPtr)
        -void publishCmd(double, double)
        -rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub
        -rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub
        -rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub
        -rclcpp::TimerBase::SharedPtr timer
        -std::vector<double> cx
        -std::vector<double> cy
        -std::vector<double> cyaw
        -std::vector<double> ck
        -double x, y, yaw, v, w
        -int target_ind
        -int oldNearestPointIndex
        -double target_vel
        -double current_vel
        -bool path_subscribe_flag
        -double goal_threshold
        -const double k
        -const double Lfc
        -const double Kp
        -const double dt
        -double minCurvature
        -double maxCurvature
        -double minVelocity
        -double maxVelocity
    }

Flowchart

flowchart TD
    A[Start] --> B[Initialize ROS 2 Node: pure_pursuit_planner]
    B --> C[Create Publishers and Subscribers]
    C --> D[Enter Timer Callback Loop]
    D --> E[Update Control]
    E --> F[Pure Pursuit Control]
    F --> G[Publish Command Velocity]
    G --> H[Check if Goal is Reached]
    H -->|No| I[Continue Path Tracking]
    H -->|Yes| J[Stop the Robot]
    I --> D
    J --> K[End]

    subgraph PurePursuitNode
        L[Constructor: Initialize Node, Topics, and Parameters]
        M[odometry_callback: Update Robot State]
        N[path_callback: Receive and Process Path]
        O[updateControl: Timer Callback for Control Update]
        P[purePursuitControl: Calculate Steering and Velocity]
        Q[publishCmd: Publish Velocity Command to cmd_vel]
    end

    L --> M
    L --> N
    M --> O
    N -.-> O
    O --> P
    P --> Q

System architecture

classDiagram
    class PathPublisher {
        +PathPublisher()
        +void loadPathData(string&)
        +void publishPath()
        -rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_
        -rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr visualize_path_pub_
        -rclcpp::TimerBase::SharedPtr timer_
        -nav_msgs::msg::Path path_
        -nav_msgs::msg::Path visualize_path_
    }
    
    class PurePursuitNode {
        +PurePursuitNode()
        -void updateControl()
        -std::pair<double, double> purePursuitControl(int&)
        -std::pair<int, double> searchTargetIndex()
        -double calcDistance(double, double) const
        -void odometry_callback(nav_msgs::msg::Odometry::SharedPtr)
        -void path_callback(nav_msgs::msg::Path::SharedPtr)
        -void publishCmd(double, double)
        -rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub
        -rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub
        -rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub
        -rclcpp::TimerBase::SharedPtr timer
        -std::vector<double> cx
        -std::vector<double> cy
        -std::vector<double> cyaw
        -std::vector<double> ck
        -double x, y, yaw, v, w
        -int target_ind
        -int oldNearestPointIndex
        -double target_vel
        -double current_vel
        -bool path_subscribe_flag
        -double goal_threshold
        -const double k
        -const double Lfc
        -const double Kp
        -const double dt
        -double minCurvature
        -double maxCurvature
        -double minVelocity
        -double maxVelocity
    }

    PathPublisher --|> PurePursuitNode: path_

    class OdometryPublisher {
        -rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub
        -rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub
        -rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr localmap_pub
        -rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr laser_range_pub
        -rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscriber
        -std::shared_ptr<tf2_ros::TransformBroadcaster> odom_broadcaster
        -std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_broadcaster_
        -rclcpp::TimerBase::SharedPtr timer_
        -nav_msgs::msg::Path path
        -double x, y, th, vx, vth
        -rclcpp::Time current_time, last_time
        +OdometryPublisher()
        -void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr)
        -void timer_callback()
        -void send_static_transform()
    }

    PurePursuitNode --|> OdometryPublisher: v, w
    OdometryPublisher --|> PurePursuitNode: x, y, th

Functional Requirements

Feature	Requirement	Reason	Specification	Implemented Functions
Path Following Control	The robot needs to efficiently move along the specified path.	To ensure the robot accurately progresses towards the target.	- Use the Pure Pursuit algorithm to calculate the angle to the target point from the robot's current position and determine the velocity and angular velocity based on that. (Implemented in `purePursuitControl`) - Search for the index of the target point on the path that is a certain distance (`Lf`) ahead of the robot. The distance dynamically changes according to the velocity. (Implemented in `searchTargetIndex`)	`updateControl`, `purePursuitControl`, `searchTargetIndex`
Velocity Command Output	The robot needs to output the computed velocity and angular velocity commands.	To ensure the robot progresses at the appropriate speed.	- Output the computed velocity (`v`) and angular velocity (`w`) as a `geometry_msgs::msg::Twist` message. (Implemented in `publishCmd`) - Stop by setting the velocity and angular velocity to zero if approaching the goal (`goal_threshold` or less). (Implemented in `publishCmd`)	`publishCmd`
Path Output	The robot needs to output the path to follow.	To provide a reference path for navigation.	- Output each point of the path (`cx[i]`, `cy[i]`) as part of a `nav_msgs::msg::Path` message. Each point is stored as a `geometry_msgs::msg::PoseStamped` message. (Implemented in `publishPath`)	`publishPath`
Odometry Reception	The robot needs to receive odometry information.	To update the current position and orientation of the robot.	- Extract the robot's current position (`x`, `y`) and orientation (`yaw`) from the received `nav_msgs::msg::Odometry` message and update the internal variables. (Implemented in `odometry_callback`)	`odometry_callback`

Detailed Design

Method Name	Purpose	Process
`updateControl`	To execute the control loop and update the velocity command and following path.	1. Call the `purePursuitControl` method to calculate the velocity and angular velocity. 2. Call the `publishCmd` method to publish the velocity command. 3. Call the `publishPath` method to publish the following path.
`purePursuitControl`	To calculate the velocity and angular velocity based on the Pure Pursuit algorithm.	1. Call the `searchTargetIndex` method to search for the index of the target point. 2. Calculate the angle to the target point. 3. Calculate the velocity and angular velocity based on the calculated angle and parameters.
`publishPath`	To publish the following path.	1. Create a `nav_msgs::msg::Path` message. 2. Add each point of the path to the `Path` message. 3. Use `path_pub` to publish the `Path` message.
`searchTargetIndex`	To search for the index of the target point.	1. Search for the index of the target point that is a certain distance (`Lf`) ahead of the robot. 2. The distance dynamically changes according to the velocity.
`calcDistance`	To calculate the distance between a point and the robot's current position.	1. Calculate the Euclidean distance between the robot's current position (`x`, `y`) and the specified point (`point_x`, `point_y`).
`odometry_callback`	Callback function for odometry information.	1. Extract the robot's current position (`x`, `y`) and orientation (`yaw`) from the received `nav_msgs::msg::Odometry` message and update the internal variables.
`publishCmd`	To publish the velocity command.	1. Create a `geometry_msgs::msg::Twist` message and set the calculated velocity (`v`) and angular velocity (`w`). 2. Use `cmd_vel_pub` to publish the `Twist` message. 3. Stop by setting the velocity and angular velocity to zero if approaching the goal (`goal_threshold` or less).

Arcanain/pure_pursuit_planner