update: LQR local planner

README.md

@@ -152,6 +152,7 @@ For the efficient operation of the motion planning system, we provide a series o
 | Planner | Version | Animation |
 |:-------:|:-------:|:---------:|
 |   **PID**   | [![Status](https://img.shields.io/badge/done-v1.1-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/local_planner/pid_planner/src/pid_planner.cpp) |           ![pid_ros.gif](assets/pid_ros.gif)            |
+|   **LQR**   | [![Status](https://img.shields.io/badge/done-v1.1-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/local_planner/lqr_planner/src/lqr_planner.cpp) |           ![lqr_ros.gif](assets/lqr_ros.gif)            |
 |   **DWA**   |     [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/local_planner/dwa_planner/src/dwa.cpp)     |           ![dwa_ros.gif](assets/dwa_ros.gif)            |
 |   **APF**   |     [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/local_planner/apf_planner/src/apf_planner.cpp)     | ![apf_ros.gif](assets/apf_ros.gif)|
 |   **RPP**   |     [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/local_planner/rpp_planner/src/rpp_planner.cpp)     | ![rpp_ros.gif](assets/rpp_ros.gif)|

src/core/local_planner/lqr_planner/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(lqr_planner)
+
+find_package(catkin REQUIRED COMPONENTS
+  angles
+  costmap_2d
+  geometry_msgs
+  nav_core
+  nav_msgs
+  navfn
+  pluginlib
+  roscpp
+  tf2_geometry_msgs
+  tf2_ros
+  base_local_planner
+  local_planner
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  CATKIN_DEPENDS local_planner
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(${PROJECT_NAME}
+  src/lqr_planner.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)

src/core/local_planner/lqr_planner/include/lqr_planner.h

@@ -0,0 +1,132 @@
+/***********************************************************
+ *
+ * @file: lqr_planner.h
+ * @breif: Contains the linear quadratic regulator (LQR) local planner class
+ * @author: Yang Haodong
+ * @update: 2024-1-12
+ * @version: 1.0
+ *
+ * Copyright (c) 2024 Yang Haodong
+ * All rights reserved.
+ * --------------------------------------------------------
+ *
+ **********************************************************/
+#ifndef LQR_PLANNER_H
+#define LQR_PLANNER_H
+
+#include <geometry_msgs/PointStamped.h>
+#include <tf2/utils.h>
+
+#include "local_planner.h"
+
+namespace lqr_planner
+{
+/**
+ * @brief A class implementing a local planner using the LQR
+ */
+class LQRPlanner : public nav_core::BaseLocalPlanner, local_planner::LocalPlanner
+{
+public:
+  /**
+   * @brief Construct a new LQR planner object
+   */
+  LQRPlanner();
+
+  /**
+   * @brief Construct a new LQR planner object
+   */
+  LQRPlanner(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros);
+
+  /**
+   * @brief Destroy the LQR planner object
+   */
+  ~LQRPlanner();
+
+  /**
+   * @brief Initialization of the local planner
+   * @param name        the name to give this instance of the trajectory planner
+   * @param tf          a pointer to a transform listener
+   * @param costmap_ros the cost map to use for assigning costs to trajectories
+   */
+  void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros);
+
+  /**
+   * @brief Set the plan that the controller is following
+   * @param orig_global_plan the plan to pass to the controller
+   * @return  true if the plan was updated successfully, else false
+   */
+  bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
+
+  /**
+   * @brief  Check if the goal pose has been achieved
+   * @return True if achieved, false otherwise
+   */
+  bool isGoalReached();
+
+  /**
+   * @brief Given the current position, orientation, and velocity of the robot, compute the velocity commands
+   * @param cmd_vel will be filled with the velocity command to be passed to the robot base
+   * @return  true if a valid trajectory was found, else false
+   */
+  bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
+
+protected:
+  /**
+   * @brief calculate the look-ahead distance with current speed dynamically
+   * @param vt  the current speed
+   * @return L  the look-ahead distance
+   */
+  double _getLookAheadDistance(double vt);
+
+  /**
+   * @brief find the point on the path that is exactly the lookahead distance away from the robot
+   * @param lookahead_dist    the lookahead distance
+   * @param robot_pose_global the robot's pose  [global]
+   * @param prune_plan        the pruned plan
+   * @return point            the lookahead point
+   */
+  geometry_msgs::PointStamped _getLookAheadPoint(double lookahead_dist, geometry_msgs::PoseStamped robot_pose_global,
+                                                 const std::vector<geometry_msgs::PoseStamped>& prune_plan);
+
+  /**
+   * @brief Prune the path, removing the waypoints that the robot has already passed and distant waypoints
+   * @param robot_pose_global the robot's pose  [global]
+   * @return pruned path
+   */
+  std::vector<geometry_msgs::PoseStamped> _prune(const geometry_msgs::PoseStamped robot_pose_global);
+
+private:
+  /**
+   * @brief Execute LQR control process
+   * @param x   state error vector
+   * @param ref reference point
+   * @return u  control vector
+   */
+  Eigen::Vector2d _lqrControl(Eigen::Vector3d x, std::vector<double> ref);
+
+protected:
+  double lookahead_time_;      // lookahead time gain
+  double min_lookahead_dist_;  // minimum lookahead distance
+  double max_lookahead_dist_;  // maximum lookahead distance
+
+private:
+  bool initialized_;                       // initialized flag
+  bool goal_reached_;                      // goal reached flag
+  tf2_ros::Buffer* tf_;                    // transform buffer
+  costmap_2d::Costmap2DROS* costmap_ros_;  // costmap(ROS wrapper)
+
+  double d_t_;         // control time interval
+  Eigen::Matrix3d Q_;  // state error matrix
+  Eigen::Matrix2d R_;  // control error matrix
+  int max_iter_;       // maximum iteration for ricatti solution
+  double eps_iter_;    // iteration ending threshold
+
+  std::vector<geometry_msgs::PoseStamped> global_plan_;
+  ros::Publisher target_pt_pub_, current_pose_pub_;
+
+  // goal parameters
+  double goal_x_, goal_y_;
+  Eigen::Vector3d goal_rpy_;
+};
+}  // namespace lqr_planner
+#endif

src/core/local_planner/lqr_planner/lqr_planner_plugin.xml

@@ -0,0 +1,8 @@
+<library path="lib/liblqr_planner">
+    <class name="lqr_planner/LQRPlanner" type="lqr_planner::LQRPlanner"
+        base_class_type="nav_core::BaseLocalPlanner">
+        <description>
+            A implementation of a local linear quadratic regulator (LQR) planner.
+        </description>
+    </class>
+</library>

src/core/local_planner/lqr_planner/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>lqr_planner</name>
+  <version>0.0.0</version>
+  <description>The lqr_planner package</description>
+  <maintainer email="[email protected]">winter</maintainer>
+  <license>GPL3</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>angles</depend>
+  <depend>costmap_2d</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_core</depend>
+  <depend>nav_msgs</depend>
+  <depend>navfn</depend>
+  <depend>pluginlib</depend>
+  <depend>roscpp</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>local_planner</depend>
+
+
+  <export>
+    <nav_core plugin="${prefix}/lqr_planner_plugin.xml" />
+
+  </export>
+</package>