Skip to content

Arcanain/path_smoother

Folders and files

NameName
Last commit message
Last commit date

Latest commit

 

History

21 Commits
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

Repository files navigation

Functional Overview

This source code implements a cubic spline path planner for autonomous vehicles or robots.

Requirements

System Requirements

  • OS : Ubuntu 22.04
  • ROS2 : Humble

How To Use

Execution Steps

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

Folder structure

path_smoother/
├── include/                     - Directory for header files
│   └── path_smoother/           - Header files for the path_smoother package
│       ├── cubic_spline_1d.hpp  - Implementation of 1D cubic spline
│       └── cubic_spline_2d.hpp  - Implementation of 2D cubic spline
├── launch/                      - Directory for ROS 2 launch files
│   └── path_smoother.py         - Launch script for the path_smoother node
├── path/                        - Directory for simulation path files
│   └── simulation_path.csv      - Path data for simulation (CSV format)
├── src/                         - Directory for source files
│   ├── cubic_spline_1d_node.cpp - Node implementation for 1D cubic spline
│   ├── cubic_spline_1d_component.cpp - Component implementation for 1D cubic spline
│   ├── cubic_spline_2d_node.cpp - Node implementation for 2D cubic spline
│   ├── cubic_spline_2d_component.cpp - Component implementation for 2D cubic spline
│   └── cubic_spline_node.cpp    - Node implementation for cubic spline calculation
├── CMakeLists.txt               - CMake build configuration file
└── package.xml                  - ROS 2 package metadata file

Interface Table

Input

Variable Name Type Description
x, y std::vector<double> Input coordinates for spline interpolation.
s_query double Query parameter for position, curvature, and yaw.

Output

Variable Name Type Description
calc_position() double, std::pair<double, double> Returns the position at a given query parameter s_query.
calc_curvature() double Returns the curvature at a given query parameter s_query.
calc_yaw() double Returns the yaw angle at a given query parameter s_query.
get_s_max() double Returns the maximum value of the parameter s.

Internal Values

Variable Name Type Description
a, b, c, d std::vector<double> Coefficients for the cubic spline interpolation.
nx int Number of data points.
s, ds std::vector<double> Parameters for the 2D spline.
sx, sy CubicSpline1D 1D splines for the x and y coordinates.
rx_, ry_, ryaw_, rk_ std::vector<double> Results of spline interpolation (position, yaw, curvature).
path_ std::vector<std::vector<double>> Path data for saving to CSV.
path_pub_ rclcpp::Publisher ROS 2 publisher for the path.
timer_ rclcpp::TimerBase ROS 2 timer for publishing the path.

Software architecture

Class Diagram

classDiagram
    class CubicSpline1D {
        +std::vector<double> a
        +std::vector<double> b
        +std::vector<double> c
        +std::vector<double> d
        +std::vector<double> x
        +std::vector<double> y
        +int nx
        +CubicSpline1D(vector<double>&, vector<double>&)
        +double calc_position(double)
        +double calc_first_derivative(double)
        +double calc_second_derivative(double)
        -Eigen::MatrixXd calc_A(vector<double>&)
        -Eigen::VectorXd calc_B(vector<double>&)
        -int search_index(double)
    }

    class CubicSpline2D {
        +CubicSpline2D(vector<double>&, vector<double>&)
        +std::pair<double, double> calc_position(double)
        +double calc_curvature(double)
        +double calc_yaw(double)
        +double get_s_max()
        +std::vector<double> s
        +std::vector<double> ds
        +CubicSpline1D sx
        +CubicSpline1D sy
        -void calc_s(vector<double>&, vector<double>&)
    }
    CubicSpline2D --|> CubicSpline1D: Contains

    class CubicSplinePathPublisher {
        +CubicSplinePathPublisher()
        +void save_csv()
        +std::vector<double> rx_
        +std::vector<double> ry_
        +std::vector<double> ryaw_
        +std::vector<double> rk_
        +bool save_flag
        -void publishPath()
        -rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_
        -rclcpp::TimerBase::SharedPtr timer_
        -std::vector<std::vector<double>> path_
    }

    CubicSplinePathPublisher ..> CubicSpline2D: Uses
    CubicSplinePathPublisher ..> PathPublisher: path_

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

Flowchart

Functional Requirements

Detailed Design