Skip to content

Arcanain/pure_pursuit_planner

Folders and files

NameName
Last commit message
Last commit date

Latest commit

 

History

28 Commits
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

Repository files navigation

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.

Peek 2024-03-30 23-45

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
    }
Loading

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

Loading

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
Loading

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