Merge branch 'master' into master

README.md

@@ -313,7 +313,7 @@ Explanation:
 |:-------:|:-------:|:---------:|
 | **ACO** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/evolutionary_planner/src/aco.cpp) | ![aco_ros.gif](assets/aco_ros.gif)  |
 | **GA**  | ![Status](https://img.shields.io/badge/develop-v1.0-red) | ![Status](https://img.shields.io/badge/gif-none-yellow) |
-| **PSO** | ![Status](https://img.shields.io/badge/develop-v1.0-red) | ![Status](https://img.shields.io/badge/gif-none-yellow) |
+| **PSO** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/evolutionary_planner/src/pso.cpp) | ![pso_ros.gif](assets/pso_ros.gif) |
 | **ABC** | ![Status](https://img.shields.io/badge/develop-v1.0-red) | ![Status](https://img.shields.io/badge/gif-none-yellow) |
 
 ## <span id="4">04. Papers
@@ -336,6 +336,7 @@ Explanation:
 
 ### Evolutionary-based Planning
 * ACO: [Ant Colony Optimization: A New Meta-Heuristic](http://www.cs.yale.edu/homes/lans/readings/routing/dorigo-ants-1999.pdf).
+* PSO: [Particle Swarm Optimization](https://ieeexplore.ieee.org/document/488968)
 
 ### Local Planning
 

src/planner/curve_generation/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(curve_generation)
+
+find_package(catkin REQUIRED COMPONENTS
+  angles
+  roscpp
+)
+
+find_package(Eigen3 REQUIRED)
+
+catkin_package(
+ INCLUDE_DIRS include
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+add_library(${PROJECT_NAME}
+  src/curve.cpp
+  src/bspline_curve.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)

src/planner/curve_generation/include/bspline_curve.h

@@ -0,0 +1,137 @@
+/***********************************************************
+ *
+ * @file: bspline_curve.h
+ * @breif: B-Spline curve generation
+ * @author: Yang Haodong
+ * @update: 2023-12-20
+ * @version: 1.0
+ *
+ * Copyright (c) 2023, Yang Haodong
+ * All rights reserved.
+ * --------------------------------------------------------
+ *
+ **********************************************************/
+#ifndef B_SPLINE_CURVE_H
+#define B_SPLINE_CURVE_H
+
+#define PARAM_MODE_UNIFORMSPACED 0
+#define PARAM_MODE_CENTRIPETAL 1
+#define PARAM_MODE_CHORDLENGTH 2
+#define SPLINE_MODE_INTERPOLATION 0
+#define SPLINE_MODE_APPROXIMATION 1
+
+#include "curve.h"
+
+namespace trajectory_generation
+{
+class BSpline : public Curve
+{
+public:
+  /**
+   * @brief Construct a new B-Spline generation object
+   * @param step        Simulation or interpolation size (default: 0.01)
+   * @param order       Degree of curve (default: 3)
+   * @param param_mode  Parameterization mode (default: PARAM_MODE_CHORDLENGTH)
+   * @param spline_mode B-Spline generation mode (default: SPLINE_MODE_INTERPOLATION)
+   */
+  BSpline();
+  BSpline(double step, int order, int param_mode, int spline_mode);
+
+  /**
+   * @brief Destroy the B-Spline generation object
+   */
+  ~BSpline();
+
+  /**
+   * @brief Running trajectory generation
+   * @param points path points
+   * @param path generated trajectory
+   * @return true if generate successfully, else failed
+   */
+  bool run(const std::vector<std::pair<double, double>> points, std::vector<std::pair<double, double>>& path);
+
+  /**
+   * @brief Calculate base function using Cox-deBoor function.
+   * @param i       The index of base function
+   * @param k       The degree of curve
+   * @param t       Parameter
+   * @param knot    Knot vector
+   * @return  Nik_t The value of base function Nik(t)
+   */
+  double baseFunction(int i, int k, double t, std::vector<double> knot);
+
+  /**
+   * @brief Calculate parameters using the `uniform spaced` or `chrod length` or `centripetal` method.
+   * @param points      Path points
+   * @return parameters The parameters of given points
+   */
+  std::vector<double> paramSelection(const std::vector<std::pair<double, double>> points);
+
+  /**
+   * @brief Generate knot vector.
+   * @param parameters The parameters of given points
+   * @param n          The number of data points
+   * @return knot The knot vector
+   */
+  std::vector<double> knotGeneration(const std::vector<double> param, int n);
+
+  /**
+   * @brief Given a set of N data points, D0, D1, ..., Dn and a degree k, find a B-spline curve of degree
+   *      k defined by N control points that passes all data points in the given order.
+   * @param points          Path points
+   * @param parameters      The parameters of given points
+   * @param knot            The knot vector
+   * @return control_points The control points
+   */
+  std::vector<std::pair<double, double>> interpolation(const std::vector<std::pair<double, double>> points,
+                                                       const std::vector<double> param, const std::vector<double> knot);
+
+  /**
+   * @brief Given a set of N data points, D0, D1, ..., Dn, a degree k, and a number H, where N > H > k >= 1,
+   *    find a B-spline curve of degree k defined by H control points that satisfies the following conditions:
+          1. this curve contains the first and last data points;
+          2. this curve approximates the data polygon in the sense of least square
+   * @param points          Path points
+   * @param parameters      The parameters of given points
+   * @param knot            The knot vector
+   * @return control_points The control points
+   */
+  std::vector<std::pair<double, double>> approximation(const std::vector<std::pair<double, double>> points,
+                                                       const std::vector<double> param, const std::vector<double> knot);
+
+  /**
+   * @brief Generate the path.
+   * @param k               The degree of curve
+   * @param knot            The knot vector
+   * @param control_points  The control points
+   * @return path The smoothed trajectory points
+   */
+  std::vector<std::pair<double, double>> generation(int k, const std::vector<double> knot,
+                                                    std::vector<std::pair<double, double>> control_pts);
+
+  /**
+   * @brief Configure the degree of the curve.
+   * @param order  The degree of curve
+   */
+  void setSplineOrder(int order);
+
+  /**
+   * @brief Configure the parameterization mode.
+   * @param param_mode  The parameterization mode
+   */
+  void setParamMode(int param_mode);
+
+  /**
+   * @brief Configure the B-Spline generation mode.
+   * @param spline_mode  The B-Spline generation mode
+   */
+  void setSPlineMode(int spline_mode);
+
+protected:
+  int order_;        // Degree of curve
+  int param_mode_;   // Parameterization mode
+  int spline_mode_;  // B-Spline generation mode
+};
+}  // namespace trajectory_generation
+
+#endif

src/planner/curve_generation/include/curve.h

@@ -0,0 +1,71 @@
+/***********************************************************
+ *
+ * @file: curve.h
+ * @breif: Trajectory generation
+ * @author: Yang Haodong
+ * @update: 2023-12-20
+ * @version: 1.0
+ *
+ * Copyright (c) 2023, Yang Haodong
+ * All rights reserved.
+ * --------------------------------------------------------
+ *
+ **********************************************************/
+#ifndef CURVE_H
+#define CURVE_H
+
+#include <vector>
+#include <cmath>
+
+namespace trajectory_generation
+{
+class Curve
+{
+public:
+  /**
+   * @brief Construct a new Curve object
+   * @param step  Simulation or interpolation size
+   */
+  Curve(double step);
+
+  /**
+   * @brief Destroy the curve generation object
+   */
+  virtual ~Curve() = default;
+
+  /**
+   * @brief Running trajectory generation
+   * @param points path points
+   * @param path generated trajectory
+   * @return true if generate successfully, else failed
+   */
+  virtual bool run(const std::vector<std::pair<double, double>> points,
+                   std::vector<std::pair<double, double>>& path) = 0;
+
+  /**
+   * @brief Calculate distance between the 2 nodes.
+   * @param n1 Node 1
+   * @param n2 Node 2
+   * @return distance between nodes
+   */
+  double dist(const std::pair<double, double>& node1, const std::pair<double, double>& node2);
+
+  /**
+   * @brief Calculate length of given path.
+   * @param path    the trajectory
+   * @return length the length of path
+   */
+  double len(std::vector<std::pair<double, double>> path);
+
+  /**
+   * @brief Configure the simulation step.
+   * @param step    Simulation or interpolation size
+   */
+  void setStep(double step);
+
+protected:
+  double step_;  // Simulation or interpolation size
+};
+}  // namespace trajectory_generation
+
+#endif

src/planner/curve_generation/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>curve_generation</name>
+  <version>0.0.0</version>
+  <description>The curve_generation package</description>
+
+  <maintainer email="[email protected]">winter</maintainer>
+
+  <license>GPL3</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>angles</depend>
+  <depend>roscpp</depend>
+
+</package>