README.md

@@ -1,2 +0,0 @@
-# project
-

Src/hybrid_astar_planner-main/.vscode/c_cpp_properties.json

@@ -0,0 +1,18 @@
+{
+  "configurations": [
+    {
+      "name": "Linux",
+      "browse": {
+        "databaseFilename": "",
+        "limitSymbolsToIncludedHeaders": true
+      },
+      "includePath": [
+        "${workspaceFolder}/**",
+        "/opt/ros/melodic/include",
+        "/opt/ros/melodic/include/ompl-1.4"
+      ]
+      
+    }
+  ],
+  "version": 4
+}

Src/hybrid_astar_planner-main/.vscode/settings.json

@@ -0,0 +1,73 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/dengpw/catkin_ws/devel/lib/python2.7/dist-packages",
+        "/opt/ros/melodic/lib/python2.7/dist-packages"
+    ],
+    "files.associations": {
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "strstream": "cpp",
+        "*.tcc": "cpp",
+        "bitset": "cpp",
+        "chrono": "cpp",
+        "complex": "cpp",
+        "cstdint": "cpp",
+        "deque": "cpp",
+        "list": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "hash_map": "cpp",
+        "hash_set": "cpp",
+        "fstream": "cpp",
+        "functional": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "new": "cpp",
+        "ostream": "cpp",
+        "numeric": "cpp",
+        "ratio": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "system_error": "cpp",
+        "thread": "cpp",
+        "cfenv": "cpp",
+        "cinttypes": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "algorithm": "cpp",
+        "iterator": "cpp",
+        "map": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "set": "cpp",
+        "string": "cpp",
+        "string_view": "cpp",
+        "*.ipp": "cpp",
+        "*.txx": "cpp",
+        "valarray": "cpp"
+    }
+}

Src/hybrid_astar_planner-main/CMakeLists.txt

@@ -0,0 +1,66 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(hybrid_astar_planner)
+
+set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_CXX_COMPILER "g++")
+
+# add_subdirectory(test_the_plugin)
+
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  pluginlib
+  costmap_2d
+  roscpp
+  nav_core
+  tf2_ros
+  ompl
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  geometry_msgs
+  pluginlib
+  roscpp
+  nav_core
+  tf2_ros
+  ompl
+)
+
+include_directories(
+  include
+  test_the_plugin/include
+  ${catkin_INCLUDE_DIRS}
+  ${OMPL_INCLUDE_DIRS}
+)
+
+# find_package(ompl REQUIRED)
+
+# if(NOT OMPL_FOUND)
+#     message(AUTHOR_WARNING,"Open Motion Planning Library not found")
+# endif(NOT OMPL_FOUND)
+
+
+add_executable(test_planner test_the_plugin/src/test.cpp test_the_plugin/src/test_plugins.cpp)
+target_link_libraries(test_planner ${catkin_LIBRARIES})
+
+add_executable(tf_test_broadcaster test_the_plugin/src/tf_broadcaster.cpp)
+target_link_libraries(tf_test_broadcaster ${catkin_LIBRARIES})
+
+add_library(${PROJECT_NAME}
+  src/planner_core.cpp 
+  src/hybrid_astar.cpp 
+  src/node2d.cpp
+  src/a_start.cpp
+  src/visualize.cpp
+  src/node3d.cpp
+  src/hybrid_astar.cpp
+  src/algorithm.cpp
+  src/dubins.cpp
+  src/ReedsShepp.cpp
+)
+target_link_libraries(${PROJECT_NAME} ${OMPL_LIBRARIES})
+
+install(FILES bgp_plugin.xml
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

Src/hybrid_astar_planner-main/LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2021, dengpw
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

Src/hybrid_astar_planner-main/README.md

@@ -0,0 +1,41 @@
+# hybrid_astar_planner
+
+This is a global planner plugin of ROS move_base package.
+
+## Introduction
+Traditional robot navigation algorithms (such as the logistics storage robot used by Jingdong or Amazon)  
+mostly use a * algorithm or Dijkstra search algorithm to planning its trajectory after the map is gridded.   
+Most of them are differential models. Obviously, this kind of path is still difficult to realize for traditional  
+four-wheel vehicles whitch has nonholonomic constraint (vehicles with Ackerman steering mechanism).  
+So I designed a hybrid a * plug-in for ROS as my graduation project
+
+## How to Use
+
+### <a name="dependencies"></a>Dependencies
+Install all dependencies
+* [Open Motion Planning Library (OMPL)](http://ompl.kavrakilab.org/)
+* [ros_map_server](http://wiki.ros.org/map_server)
+
+### <a name="setup"></a><font color=Darkorange size=5 >Setup</font>
+
+Run the following command to clone, build, and launch the package (requires a sources ROS environment):
+
+```
+sudo apt install libompl-dev \
+&& mkdir -p ~/catkin_ws/src \
+&& cd ~/catkin_ws/src \
+&& git clone https://github.com/dengpw/hybrid_astar_planner.git  \
+&& cd .. \
+&& catkin_make \
+&& source devel/setup.bash \
+&& rospack profile \
+```
+
+You can run the folling command to test whether the plug-in is install propely
+```
+&& roslaunch hybrid_astar_planner test.launch
+```
+It can run a static teajectory planner using hybrid A*   
+If everything run fine,you can use this plugin in ROS naviagation package by making a special declaration before starting the launch file:  
+    &lt; `param name="base_global_planner" value="global_planner/GlobalPlanner"` / &gt;  
+    &lt; `param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS"` / &gt;

Src/hybrid_astar_planner-main/hybrid_astar_planner_plugin.xml

@@ -0,0 +1,7 @@
+<library path="lib/libhybrid_astar_planner">
+  <class name="hybrid_astar_planner/HybridAStarPlanner" type="hybrid_astar_planner::HybridAStarPlanner" base_class_type="nav_core::BaseGlobalPlanner">
+    <description>
+      A implementation of a based planner using Hybrid A*
+    </description>
+  </class>
+</library>

Src/hybrid_astar_planner-main/include/ReedsShepp.h

@@ -0,0 +1,90 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2016, Guan-Horng Liu.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the the copyright holder nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author:  Guan-Horng Liu
+*********************************************************************/
+
+#ifndef SPACES_REEDS_SHEPP_STATE_SPACE_
+#define SPACES_REEDS_SHEPP_STATE_SPACE_
+
+#include <boost/math/constants/constants.hpp>
+#include <cassert>
+
+class ReedsSheppStateSpace
+{
+public:
+
+    /** \brief The Reeds-Shepp path segment types */
+    enum ReedsSheppPathSegmentType { RS_NOP=0, RS_LEFT=1, RS_STRAIGHT=2, RS_RIGHT=3 };
+
+    /** \brief Reeds-Shepp path types */
+    static const ReedsSheppPathSegmentType reedsSheppPathType[18][5];
+    
+    /** \brief Complete description of a ReedsShepp path */
+    class ReedsSheppPath
+    {
+    public:
+        ReedsSheppPath(const ReedsSheppPathSegmentType* type=reedsSheppPathType[0],
+            double t=std::numeric_limits<double>::max(), double u=0., double v=0.,
+            double w=0., double x=0.);
+        
+        double length() const { return totalLength_; }
+
+        /** Path segment types */
+        const ReedsSheppPathSegmentType* type_;
+        /** Path segment lengths */
+        double length_[5];
+        /** Total length */
+        double totalLength_;
+    };
+
+    ReedsSheppStateSpace(double turningRadius) : rho_(turningRadius) {}
+
+    double distance(double q0[3], double q1[3]);
+
+    std::vector<ReedsSheppPathSegmentType> type(double q0[3], double q1[3]);
+
+    void sample(double q0[3], double q1[3], double step_size, double &length, std::vector<std::vector<double> > &points);
+
+    /** \brief Return the shortest Reeds-Shepp path from SE(2) state state1 to SE(2) state state2 */
+    ReedsSheppPath reedsShepp(double q0[3], double q1[3]);
+
+protected:
+    void interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[3]);
+
+    /** \brief Turning radius */
+    double rho_;
+};
+
+#endif

Src/hybrid_astar_planner-main/include/algorithm.h

@@ -0,0 +1,32 @@
+#ifndef _ALGORITHM_H
+#define _ALGORITHM_H
+
+#include <ompl/base/spaces/ReedsSheppStateSpace.h>
+#include <ompl/base/spaces/DubinsStateSpace.h>
+#include <ompl/base/spaces/SE2StateSpace.h>
+#include <ompl/base/State.h>
+#include <ompl/base/State.h>
+#include "node3d.h"
+#include "node2d.h"
+#include <boost/heap/binomial_heap.hpp>
+#include <costmap_2d/costmap_2d.h>
+#include <geometry_msgs/PoseStamped.h>
+typedef ompl::base::SE2StateSpace::StateType State;
+namespace hybrid_astar_planner {
+// OPEN LIST AS BOOST IMPLEMENTATION
+struct CompareNodes {
+  // Sorting 3D nodes by increasing C value - the total estimated cost
+  bool operator()(const Node3D* lhs, const Node3D* rhs) const {
+    return lhs->getF() > rhs->getF();
+  }
+  // Sorting 2D nodes by increasing C value - the total estimated cost
+  bool operator()(const Node2D* lhs, const Node2D* rhs) const {
+    return lhs->getF() > rhs->getF();
+  }
+};
+Node3D* dubinsShot(Node3D& start, Node3D& goal, costmap_2d::Costmap2D* costmap);
+Node3D* reedsSheppShot(Node3D& start, Node3D& goal, costmap_2d::Costmap2D* costmap);
+void  updateH(Node3D& start, const Node3D& goal, Node2D* nodes2D, float* dubinsLookup, int width, int height, float inspireAstar);
+}
+
+#endif //end of algorithm.h

Src/hybrid_astar_planner-main/include/astar.h

@@ -0,0 +1,58 @@
+#ifndef _ASTAR_H
+#define _ASTAR_H
+
+#include <vector>
+#include "algorithm.h"
+#include "expander.h"
+#include "node2d.h"
+#include <visualization_msgs/MarkerArray.h>
+#include <ros/publisher.h>
+namespace hybrid_astar_planner {
+class Index {
+    public:
+        Index(int a,float b) {
+            i = a;
+            cost = b;
+        }
+        int i;
+        float cost;
+    };
+
+class astar : public Expander
+{
+
+    public:
+    astar(std::string frame_id, costmap_2d::Costmap2D* _costmap)
+    :Expander(frame_id, _costmap) {
+
+    }
+
+    /**
+     * @brief Find the path between the start pose and goal pose
+     * @param start the reference of start pose 
+     * @param goal the reference of goal pose 
+     * @param cells_x the number of the cells of the costmap in x axis
+     * @param cells_y the number of the cells of the costmap in y axis
+     * @param plan the refrence of plan;
+     * @return true if a valid plan was found.
+    */
+    bool calculatePath(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
+                                        int cells_x, int cells_y, std::vector<geometry_msgs::PoseStamped>& plan ,ros::Publisher& pub, visualization_msgs::MarkerArray& pathNodes );
+    ~astar(){ }
+    private:
+
+    std::vector<Node2D*> gatAdjacentPoints(int cells_x, int cells_y, const unsigned char* charMap, Node2D* pathNode2D, Node2D *point );
+
+    /**
+     * @brief transform the 2Dnode to geometry_msgs::PoseStamped
+     * @param node the ptr of node
+     * @param plan the refrence of plan
+    */
+    void nodeToPlan(Node2D* node, std::vector<geometry_msgs::PoseStamped>& plan);
+    /* data */
+};
+
+
+}//end of namespace hybrid_astar_planner
+
+#endif //the end of astar.h

Src/hybrid_astar_planner-main/include/constants.h

@@ -0,0 +1,84 @@
+#ifndef _CONSTANTS
+#define _CONSTANTS
+
+#include <cmath>
+
+namespace hybrid_astar_planner {
+
+namespace Constants {
+/// A flag to toggle reversing (true = on; false = off)
+/// 设置是否允许车辆后退的标志位 true表示可以倒退；false表示只能前进不能倒退   
+static const bool reverse = true;  
+
+/// [#] --- Limits the maximum search depth of the algorithm, possibly terminating without the solution
+/// 最大迭代次数
+static const int iterations = 30000; 
+
+/// [m] --- Uniformly adds a padding around the vehicle
+/// 膨胀范围
+static const double bloating = 0; 
+
+/// [m] --- The width of the vehicle
+static const double width = 0.18 + 2 * bloating;//车的宽度
+
+/// [m] --- The length of the vehicle
+static const double length = 0.22 + 2 * bloating;//车的长度
+
+/*
+* 车模需要转弯半径为0.75米的
+* 车身长度需要0.15m(长) * 0.16m(轮宽)
+*/
+/// [m] --- the Minimum turning radius 车辆最小转弯半径 
+static const double r = 1;
+
+/// [m] --- The number of discretizations in heading
+/// 车体朝向的离散数量
+// static const int headings = 72;
+static const int headings = 72;
+// const float dy[] = { 0,        -0.0415893,  0.0415893};
+// const float dx[] = { 0.7068582,   0.705224,   0.705224};
+// const float dt[] = { 0,         0.1178097,   -0.1178097};
+
+const float dy[] = { 0,        -0.005198,  0.005198};
+const float dx[] = { 0.0883573,   0.088153,   0.088153};
+const float dt[] = { 0,         0.1178097,   -0.1178097};
+/// [°] --- The discretization value of the heading (goal condition)
+/// 朝向离散度数(以度表示)
+static const float deltaHeadingDeg = 360 / (float)headings; 
+
+/// [c*M_PI] --- The discretization value of heading (goal condition)
+static const float deltaHeadingRad = 2 * M_PI / (float)headings; //朝向离散步长(以弧度表示)
+
+/// [c*M_PI] --- The heading part of the goal condition 
+static const float deltaHeadingNegRad = 2 * M_PI - deltaHeadingRad;
+
+/// A flag to toggle the connection of the path via Dubin's shot (true = on; false = off)
+static const bool dubinsShot = false; //切换Dubin路径的开关
+
+/// A flag to toggle the connection of the path via reedsSheppShot (true = on; false = off)
+static const bool reedsSheppShot = true; //切换Dubin路径的开关
+
+/// A flag to toggle the Dubin's heuristic, this should be false, if reversing is enabled (true = on; false = off)
+static const bool dubins = false;//Dubin路径的切换开关: 若车子可以倒退，值为false
+// ___________________
+// HEURISTIC CONSTANTS
+
+/// [#] --- A factor to ensure admissibility of the holonomic with obstacles heuristic
+static const float factor2D = sqrt(5) / sqrt(2) + 1;
+/// [#] --- A movement cost penalty for turning (choosing non straight motion primitives)
+static const float penaltyTurning = 1.05;
+/// [#] --- A movement cost penalty for reversing (choosing motion primitives > 2)
+static const float penaltyReversing = 1.5;
+/// [#] --- A movement cost penalty for change of direction (changing from primitives < 3 to primitives > 2)
+static const float penaltyCOD = 1.5;
+/// [m] --- The distance to the goal when the analytical solution (Dubin's shot) first triggers
+static const float dubinsShotDistance = 100;
+/// [m] --- The step size for the analytical solution (Dubin's shot) primarily relevant for collision checking
+static const float dubinsStepSize = 0.088;
+
+}
+
+
+}
+
+#endif

Src/hybrid_astar_planner-main/include/dubins.h

@@ -0,0 +1,218 @@
+/*!
+  \file dubins.h
+  \brief    A dubins path class for finding analytical solutions to the problem of the shortest path.
+            It has been copied from https://github.com/AndrewWalker/Dubins-Curves/ under the WTFPL. 
+            Please refer to the author in case of questions.
+  \author Andrew Walker
+*/
+// Copyright (c) 2008-2014, Andrew Walker
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+#ifndef DUBINS_H
+#define DUBINS_H
+
+// Path types
+#define LSL (0)
+#define LSR (1)
+#define RSL (2)
+#define RSR (3)
+#define RLR (4)
+#define LRL (5)
+
+// Error codes
+#define EDUBOK        (0)   // No error
+#define EDUBCOCONFIGS (1)   // Colocated configurations
+#define EDUBPARAM     (2)   // Path parameterisitation error
+#define EDUBBADRHO    (3)   // the rho value is invalid
+#define EDUBNOPATH    (4)   // no connection between configurations with this word
+
+namespace hybrid_astar_planner {
+
+// The various types of solvers for each of the path types
+typedef int (*DubinsWord)(double, double, double, double* );
+
+// A complete list of the possible solvers that could give optimal paths
+extern DubinsWord dubins_words[];
+
+typedef struct
+{
+    double qi[3];       // the initial configuration
+    double param[3];    // the lengths of the three segments
+    double rho;         // model forward velocity / model angular velocity
+    int type;           // path type. one of LSL, LSR, ...
+} DubinsPath;
+
+/**
+ * Callback function for path sampling
+ *
+ * @note the q parameter is a configuration
+ * @note the t parameter is the distance along the path
+ * @note the user_data parameter is forwarded from the caller
+ * @note return non-zero to denote sampling should be stopped
+ */
+
+/**
+ * @brief 回调函数:
+ *    q：一个构型(configuration)
+ *    t：路径长度
+ *    user_data：传递的数据
+ * 
+ */
+typedef int (*DubinsPathSamplingCallback)(double q[3], double t, void* user_data);
+
+
+/**
+ * Generate a path from an initial configuration to
+ * a target configuration, with a specified maximum turning
+ * radii
+ *
+ * A configuration is (x, y, theta), where theta is in radians, with zero
+ * along the line x = 0, and counter-clockwise is positive
+ *
+ * @param q0    - a configuration specified as an array of x, y, theta
+ * @param q1    - a configuration specified as an array of x, y, theta
+ * @param rho   - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
+ * @param path  - the resultant path
+ * @return      - non-zero on error
+ */
+
+/**
+ * @brief 从初始构型(始点)q0构建一条到q1的dubins路径，最大转向半径为rho(单位为米?)
+ * 
+ * @param q0 起点
+ * @param q1 目标点
+ * @param rho 转向半径
+ * @param path  构建的路径点
+ * @return int 返回值：非0表示出错；0表示正常
+ */
+int dubins_init( double q0[3], double q1[3], double rho, DubinsPath* path);
+
+/**
+ * Calculate the length of an initialised path
+ *
+ * @param path - the path to find the length of
+ */
+/**
+ * @brief 计算一条经过初始化阶段（归一化）的路径的长度
+ * 
+ * @param path 经过dubins_init()函数计算出来的路径点
+ * @return double 返回值表示该路径的长度
+ */
+double dubins_path_length( DubinsPath* path );
+
+/**
+ * Extract an integer that represents which path type was used
+ *
+ * @param path    - an initialised path
+ * @return        - one of LSL, LSR, RSL, RSR, RLR or LRL (ie/ 0-5 inclusive)
+ */
+/**
+ * @brief 计算一段路径的类型
+ * 
+ * @param path：路径点
+ * @return int：路径类型，整数表示LSL, LSR, RSL, RSR, RLR or LRL(用整数0-5之一表示结果)
+ */
+int dubins_path_type( DubinsPath * path );
+
+/**
+ * Calculate the configuration along the path, using the parameter t
+ *
+ * @param path - an initialised path
+ * @param t    - a length measure, where 0 <= t < dubins_path_length(path)
+ * @param q    - the configuration result
+ * @returns    - non-zero if 't' is not in the correct range
+ */
+
+/**
+ * @brief 使用参数t计算沿路径path的构型（位姿）
+ * 
+ * @param path 路径点
+ * @param t: 长度单位，范围为 0 <= t <= dubins_path_length(path)
+ * @param q: 计算结果
+ * @return int： 返回值，非0值表示出错，0表示正常
+ */
+int dubins_path_sample( DubinsPath* path, double t, double q[3]);
+
+/**
+ * Walk along the path at a fixed sampling interval, calling the
+ * callback function at each interval
+ *
+ * @param path      - the path to sample
+ * @param cb        - the callback function to call for each sample
+ * @param user_data - optional information to pass on to the callback
+ * @param stepSize  - the distance along the path for subsequent samples
+ */
+/**
+ * @brief 用固定步长在路径path进行遍历，每个区间调用回调函数callback一次
+ * 
+ * @param path 路径
+ * @param cb 回调函数
+ * @param stepSize 步长
+ * @param user_data 用户数据，用于交换数据
+ * @return int 返回值：非0表示出错；0表示正常
+ */
+int dubins_path_sample_many( DubinsPath* path, DubinsPathSamplingCallback cb, double stepSize, void* user_data );
+
+/**
+ * Convenience function to identify the endpoint of a path
+ *
+ * @param path - an initialised path
+ * @param q    - the configuration result
+ */
+/**
+ * @brief 用于提取路径的终点
+ * 
+ * @param path 路径
+ * @param q 提取的终点
+ * @return int 返回值：非0表示出错；0表示正常
+ */
+int dubins_path_endpoint( DubinsPath* path, double q[3] );
+
+/**
+ * Convenience function to extract a subset of a path
+ *
+ * @param path    - an initialised path
+ * @param t       - a length measure, where 0 < t < dubins_path_length(path)
+ * @param newpath - the resultant path
+ */
+/**
+ * @brief 提取路径的一段子集的函数
+ * 
+ * @param path 给定的路径 
+ * @param t  长度，范围 0 < t < dubins_path_length(path)
+ * @param newpath 提取的子路径
+ * @return int 返回值：非0表示出错；0表示正常
+ */
+int dubins_extract_subpath( DubinsPath* path, double t, DubinsPath* newpath );
+
+// Only exposed for testing purposes
+//构造不同类型的路径的函数
+int dubins_LSL( double alpha, double beta, double d, double* outputs );
+int dubins_RSR( double alpha, double beta, double d, double* outputs );
+int dubins_LSR( double alpha, double beta, double d, double* outputs );
+int dubins_RSL( double alpha, double beta, double d, double* outputs );
+int dubins_LRL( double alpha, double beta, double d, double* outputs );
+int dubins_RLR( double alpha, double beta, double d, double* outputs );
+void dubins_segment( double t, double qi[3], double qt[3], int type);
+int dubins_init_normalised( double alpha, double beta, double d, DubinsPath* path);
+double mod2pi( double theta );
+double fmodr( double x, double y);
+}
+
+#endif //end of dubins.h

Src/hybrid_astar_planner-main/include/expander.h

@@ -0,0 +1,35 @@
+#ifndef _EXPANDER_H
+#define _EXPANDER_H
+#include <vector>
+#include <geometry_msgs/Point.h>
+#include <costmap_2d/costmap_2d.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <boost/heap/binomial_heap.hpp>
+#include <ros/publisher.h>
+#include <visualization_msgs/MarkerArray.h>
+namespace hybrid_astar_planner {
+
+/**
+ * @brief the base class of astar or hybrid astar planner
+*/
+class Expander
+{
+
+    public:
+    Expander(std::string frame_id, costmap_2d::Costmap2D* _costmap)
+    :frame_id_(frame_id), costmap(_costmap) {
+
+    }
+
+    virtual bool calculatePath(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
+                                        int cells_x, int cells_y, std::vector<geometry_msgs::PoseStamped>& plan ,ros::Publisher& pub, visualization_msgs::MarkerArray& pathNodes) = 0;
+    virtual ~Expander() {}
+    protected:
+    std::string frame_id_;
+    costmap_2d::Costmap2D* costmap;
+    /* data */
+};
+
+
+} //end of namespace hybrid_astar_planner
+#endif //end of expander.h

Src/hybrid_astar_planner-main/include/hybrid_astar.h

@@ -0,0 +1,89 @@
+#ifndef _HYBRID_ASTAR_H
+#define _HYBRID_ASTAR_H
+
+#include <vector>
+#include "algorithm.h"
+#include "expander.h"
+#include "node3d.h"
+#include <visualization_msgs/MarkerArray.h>
+#include <ros/publisher.h>
+// #define TEST
+#define point_accuracy 0.5
+#define theta_accuracy 2
+namespace hybrid_astar_planner {
+
+class hybridAstar : public Expander
+{
+
+    public:
+    /**
+     * @brief  Default constructor for the HybridAStarPlanner object
+    */
+    hybridAstar(std::string frame_id, costmap_2d::Costmap2D* _costmap)
+    :Expander(frame_id, _costmap) {
+
+    }
+
+    /**
+     * @brief Find the path between the start pose and goal pose
+     * @param start the reference of start pose 
+     * @param goal the reference of goal pose 
+     * @param cells_x the number of the cells of the costmap in x axis
+     * @param cells_y the number of the cells of the costmap in y axis
+     * @param plan the refrence of plan;
+     * @return true if a valid plan was found.
+    */
+    bool calculatePath(
+        const geometry_msgs::PoseStamped& start, 
+        const geometry_msgs::PoseStamped& goal,
+        int cellsX, int cellsY, std::vector<geometry_msgs::PoseStamped>& plan,
+        ros::Publisher& pub, visualization_msgs::MarkerArray& pathNodes);
+    
+    /**
+     * @brief Default deconstructor for the HybridAStarPlanner object
+    */
+    ~hybridAstar(){ }
+
+    private:
+ 
+    /**
+     * @brief Get the adjacent pose of a given pose
+     * @param cells_x the number of the cells of the costmap in x axis
+     * @param cells_y the number of the cells of the costmap in y axis
+     * @param charMap 
+    */
+    std::vector<Node3D*> gatAdjacentPoints(int dir, int cells_x, int cells_y, const unsigned char* charMap, Node3D *point);
+
+    /**
+     * @brief judge whether is reach the goal pose
+     * @param node the refrence of the node
+     * @param goalPose the goal of the planner
+     * @return true if reach the goal
+    */
+    bool reachGoal(Node3D* node, Node3D* goalPose); 
+
+    /**
+     * @brief get the index of node
+     * @param x the x position axis of the node
+     * @param y the y position axis of the node
+     * @param cells_x the scale of cells in x axis
+     * @param t the depth of the 3D nodes
+     * @return the index of node 
+    */
+    int calcIndix(float x, float y, int cells_x, float t); 
+
+    /**
+     * @brief transform the 2Dnode to geometry_msgs::PoseStamped
+     * @param node the ptr of node
+     * @param plan the refrence of plan
+    */
+    void nodeToPlan(Node3D* node, std::vector<geometry_msgs::PoseStamped>& plan);
+    std::unique_ptr<GridSearch> grid_a_star_heuristic_generator_;
+};
+
+
+
+
+}//end of namespace hybrid_astar_planner
+
+#endif //the end of astar.h

Src/hybrid_astar_planner-main/include/node2d.h

@@ -0,0 +1,99 @@
+#ifndef _NODE2D_H
+#define _NODE2D_H
+
+#include <cmath>
+#include <unordered_map>
+#include <costmap_2d/costmap_2d.h>
+namespace hybrid_astar_planner {
+
+class Node2D
+{
+public:
+    Node2D(): Node2D(0, 0, 999, 0, nullptr) {}
+    Node2D(int _x , int _y, float _g = 999, float _h = 0, Node2D* _perd = nullptr):
+    x(_x), y(_y), g(_g), h(_h), perd(_perd), o(false), c(false)
+    {
+        index = -1;
+    }
+    ~Node2D(){}
+    
+    void setX(int _x) { x = _x; }
+    void setY(int _y) { y = _y; }
+    void setCost(unsigned int C) { cost = C; }
+    void setG(float _g) { g = _g; }
+    void setH(float _h) { h = _h; }
+    void setClosedSet() { c = true; o = false; }
+    void setOpenSet() { o = true; }
+    void setPerd(Node2D* _perd) { perd = _perd; }
+    void setPerd_(std::shared_ptr<Node2D> _perd) { perd_node = _perd; }
+    int getX(void) { return x; }
+    int getY(void) { return y; }
+    float getF(void) const { return g + h; }
+    float calcG(Node2D const *partent);
+    float getG() const { return g; }
+    float calcH(Node2D const *goal);
+    int getindex(int width) { return (y * width + x); }
+    int getCost() {return cost;}
+    bool isOpenSet() { return o; }
+    bool isClosedSet() { return c; }
+    Node2D* getPerd() { return perd; }
+    void SetPathCost(const float path_cost) {
+        g = path_cost;
+    }
+    private:
+        // float cost_ = 999;
+        /// the x position
+        int x;
+        /// the y position
+        int y;
+        /// the cost of current node
+        unsigned int cost;
+        /// the cost-so-far
+        float g;
+        /// the cost-to-go
+        float h;
+        /// the index of the node in the 2D array
+        int index;
+        /// the open value
+        bool o;
+        /// the closed value
+        bool c;
+        /// the discovered value
+        bool d;
+        /// the predecessor pointer
+        Node2D* perd;
+        std::shared_ptr<Node2D> perd_node = nullptr;
+};
+
+class GridSearch {
+    public:
+    GridSearch()
+    {
+
+    }
+    ~GridSearch()
+    {
+        
+    }
+    std::vector<std::shared_ptr<Node2D>> getAdjacentPoints(int cells_x,
+        int cells_y, const unsigned char* charMap, std::shared_ptr<Node2D> point);
+
+    std::unordered_map<int, std::shared_ptr<Node2D>> GenerateDpMap(
+        const double goal_x, const double goal_y, 
+        costmap_2d::Costmap2D* costmap);
+    // std::unordered_map<int, std::shared_ptr<Node2D>> dp_map_;
+    private:
+
+    // std::unordered_map<int, std::shared_ptr<Node2D>> dp_map_;
+    struct cmp {
+        // Sorting 3D nodes by increasing C value - the total estimated cost
+        bool operator()(const std::pair<int, double>& left,
+                    const std::pair<int, double>& right) const {
+            return left.second >= right.second;
+        }
+    };
+};
+
+}//end of namespace hybrid_astar_planner
+
+#endif //end of node2d.h

Src/hybrid_astar_planner-main/include/node3d.h

@@ -0,0 +1,87 @@
+#ifndef _NODE3D_H
+#define _NODE3D_H
+#include "constants.h"
+#include <cmath>
+namespace hybrid_astar_planner {
+class Node3D
+{
+public:
+    Node3D(): Node3D(0, 0, 0, 999, 0, false, nullptr) {}
+    Node3D(float _x , float _y, float t, float _g = 999, float _h = 0, bool _reverse = false, Node3D* _perd = nullptr):
+    x(_x), y(_y), g(_g), h(_h), reverse(_reverse), perd(_perd), o(false), c(false)
+    {
+        setT(t);
+        index = -1;
+    }
+    ~Node3D(){}
+    
+    void setX(float _x) { x = _x; }
+    void setY(float _y) { y = _y; }
+    void setCost(unsigned int C) { cost = C; }
+    void setT(float _t);
+    void setG(float _g) { g = _g; }
+    void setH(float _h) { h = _h; }
+    void setClosedSet() { c = true; o = false; }
+    void setOpenSet() { o = true; }
+    void setPerd(Node3D* _perd) { perd = _perd; }
+    void setReverse(bool r) { reverse = r; }
+    float getX(void) const { return x; }
+    float getY(void) const { return y; }
+    float getT(void) const { return t * Constants::deltaHeadingRad; }
+    float getTheta(void) const { return theta; }
+    float getF(void) const { return g + h; }
+    int getCost() {return cost;}
+    /**
+     * @brief caculate the G value of the node
+     * @return the G value of the node
+     * G值为状态空间中从初始节点到当前此节点的实际代价
+     *
+    */
+    float calcG(void);
+    float getG() const { return g; }
+    float calcH(Node3D const *goal);
+    int getindex(int width, int depth, float resolution, unsigned int dx, unsigned int dy) { this->index = (int(x/resolution + dx) * width + int(y/resolution + dy))*depth + t; return index;}//这里的resolution变动了
+    bool isOpenSet() { return o; }
+    bool isClosedSet() { return c; }
+    bool isReverse() { return reverse; }
+    Node3D* getPerd() { return perd; }
+    // RANGE CHECKING
+    /// Determines whether it is appropriate to find a analytical solution.
+    bool isInRange(const Node3D& goal) const;//检测是否可以分析方法找到解
+
+    // CUSTOM OPERATORS
+    /// Custom operator to compare nodes. Nodes are equal if their x and y position as well as heading is similar.
+    bool operator == (const Node3D& rhs) const;//位置相同且朝向相似则认为是同一节点
+    
+    private:
+        /// the x position
+        float x;
+        /// the y position
+        float y;
+        /// the t position
+        int t;
+        /// the cost of this node in costmap
+        unsigned int cost;
+        /// the theta of this node 
+        float theta;
+        /// the cost-so-far
+        float g;
+        /// the cost-to-go
+        float h;
+        /// the index of the node in the 3D array
+        int index;
+        /// the open value
+        bool o;
+        /// the closed value
+        bool c;
+        /// the discovered value
+        bool d;
+        /// the flag of node,Indicates whether the vehicle is in reverse
+        bool reverse;
+        /// the predecessor pointer
+        Node3D* perd;
+};
+
+}//end of namespace hybrid_astar_planner
+
+#endif //end of node3d.h

Src/hybrid_astar_planner-main/include/planner_core.h

@@ -0,0 +1,142 @@
+#ifndef _PLANNERCORE_H
+#define _PLANNERCORE_H
+/*********************************************************************
+ *
+ *  BSD 3-Clause License
+ *
+ *  Copyright (c) 2021, dengpw
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   1 Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   2 Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   3 Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived from
+ *     this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+// #define POT_HIGH 1.0e10        // unassigned cell potential
+#include <vector>
+#include <nav_msgs/GetPlan.h>
+#include <pluginlib/class_list_macros.h>
+#include <nav_core/base_global_planner.h>
+#include <visualization_msgs/MarkerArray.h>
+#include "node2d.h"
+#include "node3d.h"
+namespace hybrid_astar_planner {
+//类插件实现的多态的时候，若在头文件中定义了函数，那么就必须有这个函数的实现，否则会报错！！！
+/**
+ * @class HybridAStarPlanner
+ * @brief Provides a ROS wrapper for the HybridAStarPlanner planner which runs a fast, interpolated navigation function on a costmap.
+ */
+
+class HybridAStarPlanner : public nav_core::BaseGlobalPlanner {
+    public:
+        /**
+         * @brief  Default constructor for the HybridAStarPlanner object
+         */
+        HybridAStarPlanner();
+
+        /**
+         * @brief  Default deconstructor for the HybridAStarPlanner object
+         */
+        ~HybridAStarPlanner();
+
+        /**
+         * @brief  Initialization function for the HybridAStarPlanner
+         * @param  name The name of this planner
+         * @param  costmap_ros A pointer to the ROS wrapper of the costmap to use for planning.And the costmap is get from the topic name /map
+         */
+        void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
+
+        void initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id);
+
+        /**
+         * @brief Given a goal pose in the world, compute a plan
+         * @param start The start pose
+         * @param goal The goal pose
+         * @param plan The plan... filled by the planner
+         * @return True if a valid plan was found, false otherwise
+         */
+        bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
+                      std::vector<geometry_msgs::PoseStamped>& plan);
+   
+        /**
+         * @brief Publish the plan to RVIZ 
+         * @param path the vector contain the path
+         */
+        void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path);
+
+        /**
+         * @brief Publish the path node to RVIZ 
+         * @param path the vector contain the path
+         */
+        void publishPathNodes(const std::vector<geometry_msgs::PoseStamped>& path);
+
+        /**
+         * @brief The call back function of makeplan service
+         * @param req 
+         * @param resp the plan the planner made
+         * @return True if a valid plan was found, false otherwise
+        */
+        bool makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp);
+    protected:
+
+        bool initialized_;
+        std::string frame_id_;
+        ros::Publisher plan_pub_;
+        ros::Publisher path_vehicles_pub_;//用于在路径上发布车子位置
+        costmap_2d::Costmap2D* costmap;
+        
+    private:
+        /**
+         * @brief Clarn the visualization_msgs::Marker 清理可视化信息的标记点
+         */
+        void clearPathNodes(void);
+
+
+
+        /**
+         * @brief Check whethe the start pose is available
+         * @param start A reference to start pose
+         * @return True if the start pose is available
+        */
+        bool checkStartPose(const geometry_msgs::PoseStamped &start);
+
+        /**
+         * @brief Check whethe the goal pose is available
+         * @param goal A reference to goal pose
+         * @return True if the goal pose is available
+        */
+        bool checkgoalPose(const geometry_msgs::PoseStamped &goal);
+        visualization_msgs::MarkerArray pathNodes;//节点数据结构，用于可视化
+        double resolution;
+        ros::ServiceServer make_plan_srv_;
+        bool use_hybrid_astar;
+
+};
+
+
+} //end namespace hybrid_astar_planner
+
+#endif

Src/hybrid_astar_planner-main/include/visualize.h

@@ -0,0 +1,10 @@
+#ifndef _VISUALIZE_H
+#define _VISUALIZE_H
+#include "node3d.h"
+#include <visualization_msgs/MarkerArray.h>
+#include <ros/publisher.h>
+namespace hybrid_astar_planner {
+    void publishSearchNodes(Node3D node,ros::Publisher& pub, visualization_msgs::MarkerArray& pathNodes, int i);
+}
+
+#endif//end of visualize,h

Src/hybrid_astar_planner-main/package.xml

@@ -0,0 +1,49 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>hybrid_astar_planner</name>
+  <version>1.1.2</version>
+  <description>A hybrid_astar_planner package for move_base</description>
+  <maintainer email="dengpw@qq.com">dengpw</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_export_depend>pluginlib</build_export_depend>
+  <exec_depend>pluginlib</exec_depend>
+
+
+  <build_depend>nav_core</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>ompl</build_depend>
+  <build_depend>map_server</build_depend>
+
+  <exec_depend>nav_core</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+  <exec_depend>tf</exec_depend>
+  <exec_depend>ompl</exec_depend>
+  <exec_depend>map_server</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+      <nav_core plugin="${prefix}/hybrid_astar_planner_plugin.xml" />
+  </export>
+  
+  
+</package>

Src/hybrid_astar_planner-main/src/ReedsShepp.cpp

@@ -0,0 +1,623 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2016, Guan-Horng Liu.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the the copyright holder nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author:  Guan-Horng Liu
+*********************************************************************/
+
+
+#include "ReedsShepp.h"
+#include <boost/math/constants/constants.hpp>
+
+
+namespace
+{
+    // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.
+
+    const double pi = boost::math::constants::pi<double>();
+    const double twopi = 2. * pi;
+    const double RS_EPS = 1e-6;
+    const double ZERO = 10*std::numeric_limits<double>::epsilon();
+
+    inline double mod2pi(double x)
+    {
+        double v = fmod(x, twopi);
+        if (v < -pi)
+            v += twopi;
+        else
+            if (v > pi)
+                v -= twopi;
+        return v;
+    }
+    inline void polar(double x, double y, double &r, double &theta)
+    {
+        r = sqrt(x*x + y*y);
+        theta = atan2(y, x);
+    }
+    inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
+    {
+        double delta = mod2pi(u-v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.;
+        double t1 = atan2(eta*A - xi*B, xi*A + eta*B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
+        tau = (t2<0) ? mod2pi(t1+pi) : mod2pi(t1);
+        omega = mod2pi(tau - u + v - phi) ;
+    }
+
+    // formula 8.1 in Reeds-Shepp paper
+    inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        polar(x - sin(phi), y - 1. + cos(phi), u, t);
+        if (t >= -ZERO)
+        {
+            v = mod2pi(phi - t);
+            if (v >= -ZERO)
+            {
+                assert(fabs(u*cos(t) + sin(phi) - x) < RS_EPS);
+                assert(fabs(u*sin(t) - cos(phi) + 1 - y) < RS_EPS);
+                assert(fabs(mod2pi(t+v - phi)) < RS_EPS);
+                return true;
+            }
+        }
+        return false;
+    }
+    // formula 8.2
+    inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double t1, u1;
+        polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
+        u1 = u1*u1;
+        if (u1 >= 4.)
+        {
+            double theta;
+            u = sqrt(u1 - 4.);
+            theta = atan2(2., u);
+            t = mod2pi(t1 + theta);
+            v = mod2pi(t - phi);
+            assert(fabs(2*sin(t) + u*cos(t) - sin(phi) - x) < RS_EPS);
+            assert(fabs(-2*cos(t) + u*sin(t) + cos(phi) + 1 - y) < RS_EPS);
+            assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
+            return t>=-ZERO && v>=-ZERO;
+        }
+        return false;
+    }
+    void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+    {
+        double t, u, v, Lmin = path.length(), L;
+        if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[14], t, u, v);
+            Lmin = L;
+        }
+        if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[14], -t, -u, -v);
+            Lmin = L;
+        }
+        if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[15], t, u, v);
+            Lmin = L;
+        }
+        if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[15], -t, -u, -v);
+            Lmin = L;
+        }
+
+        if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[12], t, u, v);
+            Lmin = L;
+        }
+        if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[12], -t, -u, -v);
+            Lmin = L;
+        }
+        if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[13], t, u, v);
+            Lmin = L;
+        }
+        if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[13], -t, -u, -v);
+    }
+    // formula 8.3 / 8.4  *** TYPO IN PAPER ***
+    inline bool LpRmL(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
+        polar(xi, eta, u1, theta);
+        if (u1 <= 4.)
+        {
+            u = -2.*asin(.25 * u1);
+            t = mod2pi(theta + .5 * u + pi);
+            v = mod2pi(phi - t + u);
+            assert(fabs(2*(sin(t) - sin(t-u)) + sin(phi) - x) < RS_EPS);
+            assert(fabs(2*(-cos(t) + cos(t-u)) - cos(phi) + 1 - y) < RS_EPS);
+            assert(fabs(mod2pi(t-u+v - phi)) < RS_EPS);
+            return t>=-ZERO && u<=ZERO;
+        }
+        return false;
+    }
+    void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+    {
+        double t, u, v, Lmin = path.length(), L;
+        if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[0], t, u, v);
+            Lmin = L;
+        }
+        if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[0], -t, -u, -v);
+            Lmin = L;
+        }
+        if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[1], t, u, v);
+            Lmin = L;
+        }
+        if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
+            Lmin = L;
+        }
+
+        // backwards
+        double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
+        if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[0], v, u, t);
+            Lmin = L;
+        }
+        if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[0], -v, -u, -t);
+            Lmin = L;
+        }
+        if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[1], v, u, t);
+            Lmin = L;
+        }
+        if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[1], -v, -u, -t);
+    }
+    // formula 8.7
+    inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi*xi + eta*eta));
+        if (rho <= 1.)
+        {
+            u = acos(rho);
+            tauOmega(u, -u, xi, eta, phi, t, v);
+            assert(fabs(2*(sin(t)-sin(t-u)+sin(t-2*u))-sin(phi) - x) < RS_EPS);
+            assert(fabs(2*(-cos(t)+cos(t-u)-cos(t-2*u))+cos(phi)+1 - y) < RS_EPS);
+            assert(fabs(mod2pi(t-2*u-v - phi)) < RS_EPS);
+            return t>=-ZERO && v<=ZERO;
+        }
+        return false;
+    }
+    // formula 8.8
+    inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi*xi - eta*eta) / 16.;
+        if (rho>=0 && rho<=1)
+        {
+            u = -acos(rho);
+            if (u >= -.5 * pi)
+            {
+                tauOmega(u, u, xi, eta, phi, t, v);
+                assert(fabs(4*sin(t)-2*sin(t-u)-sin(phi) - x) < RS_EPS);
+                assert(fabs(-4*cos(t)+2*cos(t-u)+cos(phi)+1 - y) < RS_EPS);
+                assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
+                return t>=-ZERO && v>=-ZERO;
+            }
+        }
+        return false;
+    }
+    void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+    {
+        double t, u, v, Lmin = path.length(), L;
+        if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[2], t, u, -u, v);
+            Lmin = L;
+        }
+        if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
+            Lmin = L;
+        }
+        if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[3], t, u, -u, v);
+            Lmin = L;
+        }
+        if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
+            Lmin = L;
+        }
+
+        if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[2], t, u, u, v);
+            Lmin = L;
+        }
+        if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
+            Lmin = L;
+        }
+        if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[3], t, u, u, v);
+            Lmin = L;
+        }
+        if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, -u, -v);
+    }
+    // formula 8.9
+    inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
+        polar(xi, eta, rho, theta);
+        if (rho >= 2.)
+        {
+            double r = sqrt(rho*rho - 4.);
+            u = 2. - r;
+            t = mod2pi(theta + atan2(r, -2.));
+            v = mod2pi(phi - .5*pi - t);
+            assert(fabs(2*(sin(t)-cos(t))-u*sin(t)+sin(phi) - x) < RS_EPS);
+            assert(fabs(-2*(sin(t)+cos(t))+u*cos(t)-cos(phi)+1 - y) < RS_EPS);
+            assert(fabs(mod2pi(t+pi/2+v-phi)) < RS_EPS);
+            return t>=-ZERO && u<=ZERO && v<=ZERO;
+        }
+        return false;
+    }
+    // formula 8.10
+    inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
+        polar(-eta, xi, rho, theta);
+        if (rho >= 2.)
+        {
+            t = theta;
+            u = 2. - rho;
+            v = mod2pi(t + .5*pi - phi);
+            assert(fabs(2*sin(t)-cos(t-v)-u*sin(t) - x) < RS_EPS);
+            assert(fabs(-2*cos(t)-sin(t-v)+u*cos(t)+1 - y) < RS_EPS);
+            assert(fabs(mod2pi(t+pi/2-v-phi)) < RS_EPS);
+            return t>=-ZERO && u<=ZERO && v<=ZERO;
+        }
+        return false;
+    }
+    void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+    {
+        double t, u, v, Lmin = path.length() - .5*pi, L;
+        if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5*pi, u, v);
+            Lmin = L;
+        }
+        if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5*pi, -u, -v);
+            Lmin = L;
+        }
+        if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5*pi, u, v);
+            Lmin = L;
+        }
+        if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5*pi, -u, -v);
+            Lmin = L;
+        }
+
+        if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5*pi, u, v);
+            Lmin = L;
+        }
+        if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5*pi, -u, -v);
+            Lmin = L;
+        }
+        if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5*pi, u, v);
+            Lmin = L;
+        }
+        if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5*pi, -u, -v);
+            Lmin = L;
+        }
+
+        // backwards
+        double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
+        if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5*pi, t);
+            Lmin = L;
+        }
+        if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5*pi, -t);
+            Lmin = L;
+        }
+        if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5*pi, t);
+            Lmin = L;
+        }
+        if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5*pi, -t);
+            Lmin = L;
+        }
+
+        if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5*pi, t);
+            Lmin = L;
+        }
+        if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5*pi, -t);
+            Lmin = L;
+        }
+        if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5*pi, t);
+            Lmin = L;
+        }
+        if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5*pi, -t);
+    }
+    // formula 8.11 *** TYPO IN PAPER ***
+    inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
+        polar(xi, eta, rho, theta);
+        if (rho >= 2.)
+        {
+            u = 4. - sqrt(rho*rho - 4.);
+            if (u <= ZERO)
+            {
+                t = mod2pi(atan2((4-u)*xi -2*eta, -2*xi + (u-4)*eta));
+                v = mod2pi(t - phi);
+                assert(fabs(4*sin(t)-2*cos(t)-u*sin(t)-sin(phi) - x) < RS_EPS);
+                assert(fabs(-4*cos(t)-2*sin(t)+u*cos(t)+cos(phi)+1 - y) < RS_EPS);
+                assert(fabs(mod2pi(t-v-phi)) < RS_EPS);
+                return t>=-ZERO && v>=-ZERO;
+            }
+        }
+        return false;
+    }
+    void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+    {
+        double t, u, v, Lmin = path.length() - pi, L;
+        if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5*pi, u, -.5*pi, v);
+            Lmin = L;
+        }
+        if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5*pi, -u, .5*pi, -v);
+            Lmin = L;
+        }
+        if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5*pi, u, -.5*pi, v);
+            Lmin = L;
+        }
+        if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[17], -t, .5*pi, -u, .5*pi, -v);
+    }
+
+    ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
+    {
+        ReedsSheppStateSpace::ReedsSheppPath path;
+        CSC(x, y, phi, path);
+        CCC(x, y, phi, path);
+        CCCC(x, y, phi, path);
+        CCSC(x, y, phi, path);
+        CCSCC(x, y, phi, path);
+        return path;
+    }
+}
+
+const ReedsSheppStateSpace::ReedsSheppPathSegmentType
+ReedsSheppStateSpace::reedsSheppPathType[18][5] = {
+    { RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP },             // 0
+    { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP },            // 1
+    { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP },           // 2
+    { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP },           // 3
+    { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP },        // 4
+    { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP },       // 5
+    { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },        // 6
+    { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },       // 7
+    { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP },       // 8
+    { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP },        // 9
+    { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },       // 10
+    { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },        // 11
+    { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },         // 12
+    { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },         // 13
+    { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },          // 14
+    { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },        // 15
+    { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT },      // 16
+    { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT }       // 17
+};
+
+ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType* type,
+    double t, double u, double v, double w, double x)
+    : type_(type)
+{
+    length_[0] = t; length_[1] = u; length_[2] = v; length_[3] = w; length_[4] = x;
+    totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
+}
+
+
+double ReedsSheppStateSpace::distance(double q0[3], double q1[3])
+{
+    return rho_ * reedsShepp(q0, q1).length();
+}
+
+ReedsSheppStateSpace::ReedsSheppPath ReedsSheppStateSpace::reedsShepp(double q0[3], double q1[3])
+{
+    double dx = q1[0] - q0[0], dy = q1[1] - q0[1], dth = q1[2] - q0[2];
+    double c = cos(q0[2]), s = sin(q0[2]);
+    double x = c*dx + s*dy, y = -s*dx + c*dy;
+    return ::reedsShepp(x/rho_, y/rho_, dth);
+}
+
+std::vector<ReedsSheppStateSpace::ReedsSheppPathSegmentType> ReedsSheppStateSpace::type(double q0[3], double q1[3])
+{
+    ReedsSheppPath path = reedsShepp(q0, q1);
+    std::vector<ReedsSheppStateSpace::ReedsSheppPathSegmentType> type_list;
+    for (int i=0;i<5;++i)
+        type_list.push_back(path.type_[i]);
+    return type_list;
+}
+
+void ReedsSheppStateSpace::sample(double q0[3], double q1[3], double step_size, double &length, std::vector<std::vector<double> > &points)
+{
+    ReedsSheppPath path = reedsShepp(q0, q1);
+    length = rho_ * path.length();
+
+    for (double seg=0.0; seg<=length; seg+=step_size) {
+        double qnew[3] = {};
+        interpolate(q0, path, seg/rho_, qnew);
+        std::vector<double> v(qnew, qnew + sizeof qnew / sizeof qnew[0]);
+        points.push_back(v);
+    }
+    return;
+}
+
+void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[3])
+{
+
+    if (seg < 0.0) seg = 0.0;
+    if (seg > path.length()) seg = path.length();
+
+    double phi, v;
+
+    s[0] = s[1] = 0.0;
+    s[2] = q0[2];
+
+    for (unsigned int i=0; i<5 && seg>0; ++i)
+    {
+        if (path.length_[i]<0)
+        {
+            v = std::max(-seg, path.length_[i]);
+            seg += v;
+        }
+        else
+        {
+            v = std::min(seg, path.length_[i]);
+            seg -= v;
+        }
+        phi = s[2];
+        switch(path.type_[i])
+        {
+            case RS_LEFT:
+                s[0] += ( sin(phi+v) - sin(phi));
+                s[1] += (-cos(phi+v) + cos(phi));
+                s[2] = phi + v;
+                break;
+            case RS_RIGHT:
+                s[0] += (-sin(phi-v) + sin(phi));
+                s[1] += ( cos(phi-v) - cos(phi));
+                s[2] = phi - v;
+                break;
+            case RS_STRAIGHT:
+                s[0] += (v * cos(phi));
+                s[1] += (v * sin(phi));
+                break;
+            case RS_NOP:
+                break;
+        }
+    }
+
+    s[0] = s[0] * rho_ + q0[0];
+    s[1] = s[1] * rho_ + q0[1];
+}

Src/hybrid_astar_planner-main/src/a_start.cpp

@@ -0,0 +1,147 @@
+/*********************************************************************
+ *
+ *  BSD 3-Clause License
+ *
+ *  Copyright (c) 2021, dengpw
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   1 Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   2 Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   3 Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived from
+ *     this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ * 
+ *  Author:  dengpw
+ *********************************************************************/
+#include "node2d.h"
+#include "astar.h"
+#include <tf/transform_datatypes.h>
+namespace hybrid_astar_planner {
+
+bool astar::calculatePath(const geometry_msgs::PoseStamped& start, 
+    const geometry_msgs::PoseStamped& goal,
+    int cells_x, int cells_y, std::vector<geometry_msgs::PoseStamped>& plan,
+    ros::Publisher& pub, visualization_msgs::MarkerArray& pathNodes ) {
+    
+  const unsigned char* charMap = costmap->getCharMap();
+
+  int counter = 0;                            // 记录程序搜寻节点次数
+  float resolution = costmap->getResolution();// 获取代价图的分辨率
+
+  // 使用boost库中的二项堆，优化优先队列的性能
+  boost::heap::binomial_heap<Node2D*,boost::heap::compare<CompareNodes>> openSet;
+  unsigned int startx, starty, goalx, goaly;
+  // 坐标转换，将世界坐标转换为costmap使用的绝对坐标
+  costmap->worldToMap(start.pose.position.x, start.pose.position.y, startx, starty);
+  costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, goalx, goaly);
+  Node2D* pathNode2D = new Node2D[cells_x * cells_y]();  // 与代价图等大的2Dnode栅格节点
+
+  // 设置开始节点与结束节点指针，指针指向其对应2D节点图中的点位
+  Node2D* startPose = &pathNode2D[ startx * cells_y + starty ];
+  Node2D* goalPose = &pathNode2D[ goalx * cells_y + goaly ];
+  
+  // 设置起始节点和结束节点的坐标，以便后续进行比较计算
+  goalPose->setX( goalx );
+  goalPose->setY( goaly );
+  startPose->setX( startx );
+  startPose->setY( starty );
+  startPose->setG(0);
+  openSet.push( startPose );
+  startPose->setOpenSet();
+
+  Node2D* tmpStart;
+  while(openSet.size()) {
+    ++counter;
+    tmpStart = openSet.top();
+    openSet.pop();
+    //如果找到目标点则返回   
+    if(tmpStart->getX() == goalPose->getX() && tmpStart->getY() == goalPose->getY() ) {
+      std::cout << "got a plan" << std::endl;
+      nodeToPlan(tmpStart, plan);
+      std::cout << counter << std::endl;
+      delete [] pathNode2D;
+      return true;
+    }
+    std::vector<Node2D*> adjacentNodes = gatAdjacentPoints(cells_x, cells_y, charMap, pathNode2D, tmpStart );    
+    tmpStart->setClosedSet();
+
+    //下面正式开始A*算法的核心搜索部分
+    for (std::vector<Node2D*>::iterator it = adjacentNodes.begin(); it != adjacentNodes.end(); ++it) { 
+      Node2D* point = *it;
+      float g;
+      if (!point->isClosedSet()) {
+        g = point->calcG(tmpStart);
+        // 在此拓展点为初次被探索时，设置此点的G值，设置其父节点。或是以其他路径到达此点的G值更小时，重新设置此点的父节点
+        if (!point->isOpenSet() || (g < point->getG())) {
+          point->setPerd(tmpStart);
+          point->setG(g);
+          if(!point->isOpenSet()) {
+            point->calcH(goalPose);                 // 计算此点距离目标节点距离（作为启发值）
+            point->setOpenSet();                    // 将此拓展点加入开集合中
+          }
+          openSet.push(point);
+        }
+      }
+    }
+  }
+
+  delete [] pathNode2D;       //删除产生的Node2D节点
+  return false;               //搜索失败
+}
+
+std::vector<Node2D*> astar::gatAdjacentPoints(int cells_x, int cells_y, const unsigned char* charMap, Node2D* pathNode2D, Node2D *point ) {
+    std::vector<Node2D*> adjacentNodes;
+  for (int x = point->getX() - 1; x <= point->getX() + 1; ++x) {
+    for (int y = point->getY() - 1; y <= point->getY() + 1; ++y) {
+      if (charMap[x  + y* cells_x] <= 1) {
+        pathNode2D[x * cells_y + y].setX(x);
+        pathNode2D[x * cells_y + y].setY(y);
+        adjacentNodes.push_back(&pathNode2D[x * cells_y + y]);
+      }
+    }
+  }//end of for
+
+  return adjacentNodes;
+}
+
+void astar::nodeToPlan(Node2D* node, std::vector<geometry_msgs::PoseStamped>& plan) {
+  Node2D* tmpPtr = node;
+  geometry_msgs::PoseStamped tmpPose;
+  tmpPose.header.stamp = ros::Time::now();   
+  //参数后期处理，发布到RViz上进行可视化
+  tmpPose.pose.orientation = tf::createQuaternionMsgFromYaw(0);
+  std::vector<geometry_msgs::PoseStamped> replan;
+  while(tmpPtr!=nullptr) {
+    costmap->mapToWorld(tmpPtr->getX(), tmpPtr->getY(), tmpPose.pose.position.x, tmpPose.pose.position.y);
+    tmpPose.header.frame_id = frame_id_;
+    replan.push_back(tmpPose);
+    tmpPtr = tmpPtr->getPerd();
+  }
+  int size = replan.size();
+  for (int i = 0;i < size; ++i) {
+    plan.push_back(replan[size - i -1 ]);
+  }
+}
+
+}// namespace hybrid_astar_planner

Src/hybrid_astar_planner-main/src/algorithm.cpp

@@ -0,0 +1,201 @@
+/*********************************************************************
+ *
+ *  BSD 3-Clause License
+ *
+ *  Copyright (c) 2021, dengpw
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   1 Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   2 Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   3 Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived from
+ *     this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ * 
+ *  Author:  dengpw
+ *********************************************************************/
+#include "algorithm.h"
+#include "dubins.h"
+#include "ReedsShepp.h"
+#include <iostream>
+#include <tf/transform_datatypes.h>
+// #include <geometry_msgs/PoseStamped.h>
+// #include <costmap_2d/costmap_2d.h>
+namespace hybrid_astar_planner {
+  
+std::vector<Node2D*> gatAdjacentPoints(int cells_x, int cells_y, const unsigned char* charMap, Node2D* pathNode2D, Node2D *point ) {
+  std::vector<Node2D*> adjacentNodes;
+  for (int x = point->getX() - 1; x <= point->getX() + 1; ++x) {
+    for (int y = point->getY() - 1; y <= point->getY() + 1; ++y) {
+      if (charMap[x  + y* cells_x] < 253) {
+        pathNode2D[x * cells_y + y].setX(x);
+        pathNode2D[x * cells_y + y].setY(y);
+        pathNode2D[x * cells_y + y].setCost(charMap[x  + y* cells_x]);
+        adjacentNodes.push_back(&pathNode2D[x * cells_y + y]);
+      }
+    }
+  }//end of for
+
+  return adjacentNodes;
+}
+void nodeToPlan(Node2D* node, std::vector<geometry_msgs::PoseStamped>& plan, costmap_2d::Costmap2D* costmap, std::string frame_id_) {
+  Node2D* tmpPtr = node;
+  geometry_msgs::PoseStamped tmpPose;
+  tmpPose.header.stamp = ros::Time::now();   
+  //参数后期处理，发布到RViz上进行可视化
+  tmpPose.pose.orientation = tf::createQuaternionMsgFromYaw(0);
+      std::vector<geometry_msgs::PoseStamped> replan;
+  while(tmpPtr!=nullptr) {
+    costmap->mapToWorld(tmpPtr->getX(), tmpPtr->getY(), tmpPose.pose.position.x, tmpPose.pose.position.y);
+    tmpPose.header.frame_id = frame_id_;
+    replan.push_back(tmpPose);
+    tmpPtr = tmpPtr->getPerd();
+  }
+  int size = replan.size();
+  for (int i = 0;i < size; ++i) {
+    plan.push_back(replan[size - i -1 ]);
+  }
+}
+
+void  updateH(Node3D& start, const Node3D& goal, Node2D* nodes2D, 
+    float* dubinsLookup, int width, int height, float inspireAstar) {
+  float reedsSheppCost = 0;
+  float twoDCost = 0;
+  float twoDoffset = 0;
+  #ifdef use_ReedsShepp_heuristic
+  // 假如车子可以后退，则可以启动Reeds-Shepp 算法
+  if (Constants::reverse && !Constants::dubins) {
+    //reeds_shepp算法还还存在问题，启用可能会造成搜寻路径增加等问题
+
+    ompl::base::ReedsSheppStateSpace reedsSheppPath(Constants::r);
+    State* rsStart = (State*)reedsSheppPath.allocState();
+    State* rsEnd = (State*)reedsSheppPath.allocState();
+    rsStart->setXY(start.getX(), start.getY());
+    rsStart->setYaw(start.getT());
+    rsEnd->setXY(goal.getX(), goal.getY());
+    rsEnd->setYaw(goal.getT());
+    reedsSheppCost = reedsSheppPath.distance(rsStart, rsEnd) * 1.1 + 
+      0.04 * start.getCost();
+
+  }
+  #endif
+  start.setH(std::max(reedsSheppCost, inspireAstar));//将两个代价中的最大值作为启发式值
+}
+
+
+Node3D* dubinsShot(Node3D& start, Node3D& goal, costmap_2d::Costmap2D* costmap) {
+  // start
+  double q0[] = { start.getX(), start.getY(), start.getT() };
+  // goal
+  double q1[] = { goal.getX(), goal.getY(), goal.getT() };
+  // initialize the path
+  DubinsPath path;
+  // calculate the path
+  dubins_init(q0, q1, Constants::r, &path);
+
+  unsigned int poseX, poseY;
+
+  int i = 0;
+  float x = 0.f;
+  float length = dubins_path_length(&path);
+  // printf("the length of dubins %f",length);
+  Node3D* dubinsNodes = new Node3D [(int)(length / Constants::dubinsStepSize) + 1];
+
+  while (x <  length) {//这是跳出循环的条件之一：生成的路径没有达到所需要的长度
+    double q[3];
+    dubins_path_sample(&path, x, q);
+    dubinsNodes[i].setX(q[0]);
+    dubinsNodes[i].setY(q[1]);
+    dubinsNodes[i].setT(q[2]);
+
+    // collision check
+    //跳出循环的条件之二：生成的路径存在碰撞节点
+    costmap->worldToMap(dubinsNodes[i].getX(), dubinsNodes[i].getY(), poseX, poseY);
+    if (costmap->getCost(poseX, poseY) <= 1) {
+
+      // set the predecessor to the previous step
+      if (i > 0) {
+        dubinsNodes[i].setPerd(&dubinsNodes[i - 1]);
+      } else {
+        dubinsNodes[i].setPerd(&start);
+      }
+
+      if (&dubinsNodes[i] == dubinsNodes[i].getPerd()) {
+        std::cout << "looping shot";
+      }
+
+      x += Constants::dubinsStepSize;
+      i++;
+    } else {
+      delete [] dubinsNodes;
+      return nullptr;
+    } 
+  }
+  //返回末节点，通过getPred()可以找到前一节点。
+  return &dubinsNodes[i - 1];
+}
+
+Node3D* reedsSheppShot(Node3D& start, Node3D& goal, costmap_2d::Costmap2D* costmap) {
+
+  ReedsSheppStateSpace rs_planner(Constants::r);
+  double length = -1;
+  unsigned int poseX, poseY;
+  // start
+  double q0[] = { start.getX(), start.getY(), start.getT() };
+  // goal
+  double q1[] = { goal.getX(), goal.getY(), goal.getT() };
+  std::vector<std::vector<double> > rs_path;
+  rs_planner.sample(q0, q1, Constants::dubinsStepSize, length, rs_path);
+  Node3D* dubinsNodes = new Node3D [(int)(length / Constants::dubinsStepSize) + 1];
+  int i = 0;
+  dubinsNodes[1].setPerd(&start);
+  for (auto &point_itr : rs_path) {
+    dubinsNodes[i].setX(point_itr[0]);
+    dubinsNodes[i].setY(point_itr[1]);
+    dubinsNodes[i].setT(point_itr[2]);
+
+    //collision check
+    costmap->worldToMap(dubinsNodes[i].getX(), dubinsNodes[i].getY(), poseX, poseY);
+    if (costmap->getCost(poseX, poseY) < 100) {
+      // set the predecessor to the previous step
+      if (i > 1) {
+        dubinsNodes[i].setPerd(&dubinsNodes[i - 1]);
+      } 
+      // else {
+      //   dubinsNodes[i].setPerd(&start);
+      // }
+      if (&dubinsNodes[i] == dubinsNodes[i].getPerd()) {
+        std::cout << "looping shot";
+      }
+      i++;
+    } else {
+      delete [] dubinsNodes;
+      return nullptr;
+    } 
+  }
+  //返回末节点，通过getPred()可以找到前一节点。
+  return &dubinsNodes[i - 1];
+
+}
+
+} //namespace hybrid_astar_planner

Src/hybrid_astar_planner-main/src/dubins.cpp

@@ -0,0 +1,448 @@
+// Copyright (c) 2008-2014, Andrew Walker
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#ifndef DUBINS_CPP
+#define DUBINS_CPP
+
+#include "dubins.h"
+#define _USE_MATH_DEFINES // for C++
+#include <math.h>
+#include <assert.h>
+
+#define EPSILON (10e-10)
+
+#define LSL (0)
+#define LSR (1)
+#define RSL (2)
+#define RSR (3)
+#define RLR (4)
+#define LRL (5)
+
+// The three segment types a path can be made up of
+#define L_SEG (0)
+#define S_SEG (1)
+#define R_SEG (2)
+
+namespace hybrid_astar_planner{
+
+// The segment types for each of the Path types
+const int DIRDATA[][3] = {
+    { L_SEG, S_SEG, L_SEG },
+    { L_SEG, S_SEG, R_SEG },
+    { R_SEG, S_SEG, L_SEG },
+    { R_SEG, S_SEG, R_SEG },
+    { R_SEG, L_SEG, R_SEG },
+    { L_SEG, R_SEG, L_SEG }
+};
+
+//函数指针数组，可以指向不同的函数
+//原型为 typedef int (*DubinsWord)(double, double, double, double* );
+DubinsWord dubins_words[] = {
+    dubins_LSL,
+    dubins_LSR,
+    dubins_RSL,
+    dubins_RSR,
+    dubins_RLR,
+    dubins_LRL,
+};
+
+//这里定义宏展开，方便简化代码
+#define UNPACK_INPUTS(alpha, beta)     \
+    double sa = sin(alpha);            \
+    double sb = sin(beta);             \
+    double ca = cos(alpha);            \
+    double cb = cos(beta);             \
+    double c_ab = cos(alpha - beta);   \
+
+//定义宏展开，方便简化代码
+#define PACK_OUTPUTS(outputs)       \
+    outputs[0]  = t;                \
+    outputs[1]  = p;                \
+    outputs[2]  = q;
+
+/**
+ * Floating point modulus suitable for rings
+ *
+ * fmod doesn't behave correctly for angular quantities, this function does
+ */
+/**
+ * 两浮点数取模
+ */
+double fmodr( double x, double y)
+{
+    return x - y*floor(x/y);
+}
+
+/**
+ * @brief 将角度范围限制在（-2pi, 2*pi）之间
+ * 
+ * @param theta 角度
+ * @return double 计算结果
+ */
+double mod2pi( double theta )
+{
+    return fmodr( theta, 2 * M_PI );
+}
+
+/**
+ * @brief 根据alpha, beta, d三个参数找出Dubins类型(LSL, LSR, RSL, RSR, RLR or LRL)
+ *        并将每段曲线的线段长度（代价）存放到path->param和path->type中
+ *       注：这是一种简单的穷举尝试策略，与文章“Classification of the Dubins set”描述的策略不同。
+ * 
+ * @param alpha ：角度参数，表示起点朝向
+ * @param beta ：角度参数，表示终点朝向
+ * @param d ：起点和终点的距离
+ * @param path 存放结果，用到两个: path->param和path->type
+ * @return int 返回值：非0值表示出错；0表示正常
+ */
+int dubins_init_normalised( double alpha, double beta, double d, DubinsPath* path)
+{
+    double best_cost = INFINITY;
+    int    best_word;
+    int    i;
+
+    best_word = -1;
+    for( i = 0; i < 6; i++ ) {
+        double params[3];
+        //分别调用不同的Dubins函数曲线计算得到t,p, q,存放于params
+        int err = dubins_words[i](alpha, beta, d, params);
+        if(err == EDUBOK) {
+            double cost = params[0] + params[1] + params[2];//三段线段的长度作为代价
+            if(cost < best_cost) {//将最好的结果保存到path->param看
+                best_word = i;
+                best_cost = cost;
+                path->param[0] = params[0];
+                path->param[1] = params[1];
+                path->param[2] = params[2];
+                path->type = i;
+            }
+        }
+    }
+
+    if(best_word == -1) {
+        return EDUBNOPATH;
+    }
+    path->type = best_word;
+    return EDUBOK;
+}
+
+/**
+ * @brief 依据始点、末点位置和最小转弯半径生成一条Dubin PATH
+ * 
+ * @param q0 始点
+ * @param q1 末点
+ * @param rho 最小转弯半径
+ * @param path 生成的路径
+ * @return int 
+ */
+int dubins_init( double q0[3], double q1[3], double rho, DubinsPath* path )
+{
+    int i;
+    double dx = q1[0] - q0[0];
+    double dy = q1[1] - q0[1];
+    double D = sqrt( dx * dx + dy * dy );
+    double d = D / rho;//将两点距离除以最小转弯半径得到归一化距离d
+    if( rho <= 0. ) {
+        return EDUBBADRHO;
+    }
+    double theta = mod2pi(atan2( dy, dx ));
+    double alpha = mod2pi(q0[2] - theta);
+    double beta  = mod2pi(q1[2] - theta);//计算坐标原点移到第一个点后，始点和终点的朝向
+    for( i = 0; i < 3; i ++ ) {//将起点放入path->qi变量
+        path->qi[i] = q0[i];
+    }
+    path->rho = rho;//最小转弯半径
+
+    return dubins_init_normalised( alpha, beta, d, path );
+}
+
+int dubins_LSL( double alpha, double beta, double d, double* outputs )
+{
+    UNPACK_INPUTS(alpha, beta);
+    double tmp0 = d+sa-sb;
+    double p_squared = 2 + (d*d) -(2*c_ab) + (2*d*(sa - sb));
+    if( p_squared < 0 ) {
+        return EDUBNOPATH;
+    }
+    double tmp1 = atan2( (cb-ca), tmp0 );
+    double t = mod2pi(-alpha + tmp1 );
+    double p = sqrt( p_squared );
+    double q = mod2pi(beta - tmp1 );
+    PACK_OUTPUTS(outputs);
+    return EDUBOK;
+}
+
+int dubins_RSR( double alpha, double beta, double d, double* outputs )
+{
+    UNPACK_INPUTS(alpha, beta);
+    double tmp0 = d-sa+sb;
+    double p_squared = 2 + (d*d) -(2*c_ab) + (2*d*(sb-sa));
+    if( p_squared < 0 ) {
+        return EDUBNOPATH;
+    }
+    double tmp1 = atan2( (ca-cb), tmp0 );
+    double t = mod2pi( alpha - tmp1 );
+    double p = sqrt( p_squared );
+    double q = mod2pi( -beta + tmp1 );
+    PACK_OUTPUTS(outputs);
+    return EDUBOK;
+}
+
+int dubins_LSR( double alpha, double beta, double d, double* outputs )
+{
+    UNPACK_INPUTS(alpha, beta);
+    double p_squared = -2 + (d*d) + (2*c_ab) + (2*d*(sa+sb));
+    if( p_squared < 0 ) {
+        return EDUBNOPATH;
+    }
+    double p    = sqrt( p_squared );
+    double tmp2 = atan2( (-ca-cb), (d+sa+sb) ) - atan2(-2.0, p);
+    double t    = mod2pi(-alpha + tmp2);
+    double q    = mod2pi( -mod2pi(beta) + tmp2 );
+    PACK_OUTPUTS(outputs);
+    return EDUBOK;
+}
+
+int dubins_RSL( double alpha, double beta, double d, double* outputs )
+{
+    UNPACK_INPUTS(alpha, beta);
+    double p_squared = (d*d) -2 + (2*c_ab) - (2*d*(sa+sb));
+    if( p_squared< 0 ) {
+        return EDUBNOPATH;
+    }
+    double p    = sqrt( p_squared );
+    double tmp2 = atan2( (ca+cb), (d-sa-sb) ) - atan2(2.0, p);
+    double t    = mod2pi(alpha - tmp2);
+    double q    = mod2pi(beta - tmp2);
+    PACK_OUTPUTS(outputs);
+    return EDUBOK;
+}
+
+int dubins_RLR( double alpha, double beta, double d, double* outputs )
+{
+    UNPACK_INPUTS(alpha, beta);
+    double tmp_rlr = (6. - d*d + 2*c_ab + 2*d*(sa-sb)) / 8.;
+    if( fabs(tmp_rlr) > 1) {
+        return EDUBNOPATH;
+    }
+    double p = mod2pi( 2*M_PI - acos( tmp_rlr ) );
+    double t = mod2pi(alpha - atan2( ca-cb, d-sa+sb ) + mod2pi(p/2.));
+    double q = mod2pi(alpha - beta - t + mod2pi(p));
+    PACK_OUTPUTS( outputs );
+    return EDUBOK;
+}
+
+int dubins_LRL( double alpha, double beta, double d, double* outputs )
+{
+    UNPACK_INPUTS(alpha, beta);
+    double tmp_lrl = (6. - d*d + 2*c_ab + 2*d*(- sa + sb)) / 8.;
+    if( fabs(tmp_lrl) > 1) {
+        return EDUBNOPATH;
+    }
+    double p = mod2pi( 2*M_PI - acos( tmp_lrl ) );
+    double t = mod2pi(-alpha - atan2( ca-cb, d+sa-sb ) + p/2.);
+    double q = mod2pi(mod2pi(beta) - alpha -t + mod2pi(p));
+    PACK_OUTPUTS( outputs );
+    return EDUBOK;
+}
+
+/**
+ * @brief 计算路径长度
+ * 
+ * @param path ：三个参数表示角度
+ * @return double ：返回值，表示路径长度
+ */
+double dubins_path_length( DubinsPath* path )
+{
+    double length = 0.;
+    length += path->param[0];
+    length += path->param[1];
+    length += path->param[2];
+    length = length * path->rho;
+    return length;
+}
+
+/**
+ * @brief 返回路径类型
+ * 
+ * @param path 输入：路径
+ * @return int 返回值：路径类型
+ */
+int dubins_path_type( DubinsPath* path ) {
+    return path->type;
+}
+
+/**
+ * @brief 计算下一段Dubins路径段的位置
+ * 
+ * @param t 角度 
+ * @param qi 该段起始位置
+ * @param qt 该段终点位置
+ * @param type 路径类型
+ */
+void dubins_segment( double t, double qi[3], double qt[3], int type)
+{
+    assert( type == L_SEG || type == S_SEG || type == R_SEG );
+    //Shkel A M, Lumelsky V. Classification of the Dubins set[J]. Robotics and Autonomous Systems, 2001, 34(4): 179-202.
+    if( type == L_SEG ) {//公式 (1)的第一个式子，此处经过归一化后v=1
+        qt[0] = qi[0] + sin(qi[2]+t) - sin(qi[2]);
+        qt[1] = qi[1] - cos(qi[2]+t) + cos(qi[2]);
+        qt[2] = qi[2] + t;
+    }
+    else if( type == R_SEG ) {//公式(1)的第二个式子，此处经过归一化后v=1
+        qt[0] = qi[0] - sin(qi[2]-t) + sin(qi[2]);
+        qt[1] = qi[1] + cos(qi[2]-t) - cos(qi[2]);
+        qt[2] = qi[2] - t;
+    }
+    else if( type == S_SEG ) {//公式(1)的第三个式子，此处经过归一化后v=1
+        qt[0] = qi[0] + cos(qi[2]) * t;
+        qt[1] = qi[1] + sin(qi[2]) * t;
+        qt[2] = qi[2];
+    }
+}
+
+/**
+ * @brief 对一段路径进行采样(根据路径长度t和路径参数path,生成长度为t的节点的位置)
+ * 
+ * @param path 输入路径参数
+ * @param t 路径长度t
+ * @param q 采样结果(线段终点的位置)
+ * @return int 返回值：非0表示出错，0表示正常
+ */
+int dubins_path_sample( DubinsPath* path, double t, double q[3] )
+{
+    if( t < 0 || t >= dubins_path_length(path) ) {
+        // error, parameter out of bounds
+        return EDUBPARAM;
+    }
+
+    // tprime is the normalised variant of the parameter t
+    double tprime = t / path->rho;
+
+    // In order to take rho != 1 into account this function needs to be more complex
+    // than it would be otherwise. The transformation is done in five stages.
+    //
+    // 1. translate the components of the initial configuration to the origin
+    // 2. generate the target configuration
+    // 3. transform the target configuration
+    //      scale the target configuration
+    //      translate the target configration back to the original starting point
+    //      normalise the target configurations angular component
+
+    // The translated initial configuration
+    // 将路径放在原点(0, 0)，此时，只需要将角度保留即可
+    double qi[3] = { 0, 0, path->qi[2] };
+
+    // Generate the target configuration
+    // 生成中间点的位置
+    const int* types = DIRDATA[path->type];
+    double p1 = path->param[0];//路径的第一个角度
+    double p2 = path->param[1];//路径的第二个角度
+    double q1[3]; // end-of segment 1
+    double q2[3]; // end-of segment 2
+    //从第qi点为起点，根据类型types[0]，生成后一个点q1的configuration（即计算下一个节点的位置）
+    dubins_segment( p1,      qi,    q1, types[0] );
+    //从第q1点为起点，根据类型types[1]，生成后一个点q2的configuration（即计算下一个节点的位置）
+    dubins_segment( p2,      q1,    q2, types[1] );
+    //生成q点的configuration
+    if( tprime < p1 ) {
+        dubins_segment( tprime, qi, q, types[0] );
+    }
+    else if( tprime < (p1+p2) ) {
+        dubins_segment( tprime-p1, q1, q,  types[1] );
+    }
+    else {
+        dubins_segment( tprime-p1-p2, q2, q,  types[2] );
+    }
+
+    // scale the target configuration, translate back to the original starting point
+    q[0] = q[0] * path->rho + path->qi[0];
+    q[1] = q[1] * path->rho + path->qi[1];
+    q[2] = mod2pi( q[2] );
+
+    return 0;
+}
+
+/**
+ * @brief 通过回调函数多次调用dubins_path_sample
+ * 
+ * @param path 
+ * @param cb 
+ * @param stepSize 
+ * @param user_data 
+ * @return int 
+ */
+int dubins_path_sample_many( DubinsPath* path, DubinsPathSamplingCallback cb, double stepSize, void* user_data )
+{
+    double x = 0.0;
+    double length = dubins_path_length(path);
+    while( x <  length ) {
+        double q[3];
+        dubins_path_sample( path, x, q );
+        int retcode = cb(q, x, user_data);
+        if( retcode != 0 ) {
+            return retcode;
+        }
+        x += stepSize;
+    }
+    return 0;
+}
+
+/**
+ * @brief 根据路径参数path, 返回线段末节点参数
+ * 
+ * @param path 
+ * @param q 
+ * @return int 
+ */
+int dubins_path_endpoint( DubinsPath* path, double q[3] )
+{
+    // TODO - introduce a new constant rather than just using EPSILON
+    return dubins_path_sample( path, dubins_path_length(path) - EPSILON, q );
+}
+
+/**
+ * @brief 将一段路径path，按路径长度t，划分为多个子线段。
+ * 
+ * @param path 输入的路径
+ * @param t 子线段长度
+ * @param newpath 生成的新的路径
+ * @return int 
+ */
+int dubins_extract_subpath( DubinsPath* path, double t, DubinsPath* newpath )
+{
+    // calculate the true parameter
+    double tprime = t / path->rho;
+
+    // copy most of the data
+    newpath->qi[0] = path->qi[0];
+    newpath->qi[1] = path->qi[1];
+    newpath->qi[2] = path->qi[2];
+    newpath->rho   = path->rho;
+    newpath->type  = path->type;
+
+    // fix the parameters
+    newpath->param[0] = fmin( path->param[0], tprime );
+    newpath->param[1] = fmin( path->param[1], tprime - newpath->param[0]);
+    newpath->param[2] = fmin( path->param[2], tprime - newpath->param[0] - newpath->param[1]);
+    return 0;
+}
+}
+#endif

Src/hybrid_astar_planner-main/src/hybrid_astar.cpp

@@ -0,0 +1,280 @@
+/*********************************************************************
+ *
+ *  BSD 3-Clause License
+ *
+ *  Copyright (c) 2021, dengpw
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   1 Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   2 Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   3 Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived from
+ *     this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ * 
+ *  Author:  dengpw
+ *  Email: dengpw@stu.xmu.edu.cn
+ *********************************************************************/
+#include "node3d.h"
+#include <math.h>
+#include "constants.h"
+#include "hybrid_astar.h"
+#include <tf/transform_datatypes.h>
+#include "visualize.h"
+// #define SearchingVisulize
+// #define debug_mode
+namespace hybrid_astar_planner {
+
+bool hybridAstar::calculatePath(
+    const geometry_msgs::PoseStamped& start, 
+    const geometry_msgs::PoseStamped& goal,
+    int cellsX, int cellsY, std::vector<geometry_msgs::PoseStamped>& plan,
+    ros::Publisher& pub, visualization_msgs::MarkerArray& pathNodes) {
+  #ifdef debug_mode
+  ros::Time t0 = ros::Time::now();
+  #endif
+  // 初始化优先队列，未来改善混合A*算法性能，这里使用的是二项堆优先队列
+  boost::heap::binomial_heap<Node3D*,boost::heap::compare<CompareNodes>> openSet;
+  // 初始化并创建一些参数。
+  // 创建charMap,charMap中存储的是地图的障碍物信息
+  const unsigned char* charMap = costmap->getCharMap(); 
+
+  /****************************************************************
+  * 分辨率太高会导致混合A*计算量过高，且意义不大, 因此
+  * 这里把resolution写死，由于本人能力有限暂时此功，其实是无奈之举，
+  * 未来需要在后续的代码中加入能够自动调节分辨率的功能，增加代码鲁棒性
+  ****************************************************************/
+  float resolution = 0.125;//costmap->getResolution()
+  unsigned int originalX, originalY, goalX, goalY;
+  int cells_x,cells_y;
+  cells_x = cellsX/2.5;
+  cells_y = cellsY/2.5;
+  int counter = 0;
+  int dir;
+  int iPred, iSucc;
+  float t, g;
+  unsigned int dx,dy;
+  unsigned int goal_x, goal_y, start_x, start_y;
+  costmap->worldToMap(0, 0, dx, dy);
+  dx = dx/2.5;
+  dy = dy/2.5;
+  // t为目标点朝向
+  t = tf::getYaw(start.pose.orientation);
+  Node3D* startPose = new Node3D(start.pose.position.x, start.pose.position.y, t , 0, 0, false, nullptr);
+
+  costmap->worldToMap(goal.pose.position.x, 
+                      goal.pose.position.y, goal_x, goal_y);
+  costmap->worldToMap(start.pose.position.x, 
+                      start.pose.position.y, start_x, start_y);
+
+
+  //************************************************
+  // 建立启发势场取决于地图大小,231(nodes/ms)
+  std::unordered_map<int, std::shared_ptr<Node2D>> dp_map = 
+      grid_a_star_heuristic_generator_->GenerateDpMap(goal_x, goal_y, costmap);    
+  ros::Time end_time = ros::Time::now();
+  #ifdef debug_mode
+  ros::Time t1 = ros::Time::now();
+  ros::Duration d(end_time - t0);
+  std::cout << "generate heuristic map costed " << d * 1000 << " ms" <<std::endl;
+  #endif
+  t = tf::getYaw(goal.pose.orientation);
+  Node3D* goalPose = new Node3D(goal.pose.position.x, goal.pose.position.y, t , 999, 0, false, nullptr);
+  std::unordered_map<int, Node3D*> open_set;
+  std::unordered_map<int, Node3D*> closed_set;
+
+  if (Constants::reverse) {
+      dir = 6;
+  }
+  else {
+      dir = 3;
+  }
+  open_set.emplace(startPose->getindex(cells_x ,Constants::headings, resolution, dx, dy), startPose);
+  openSet.push(startPose);
+  Node3D* tmpNode;
+  Node3D* nSucc;
+
+  while(openSet.size() && counter < Constants::iterations) {
+    ++counter;
+    // 根据混合A*算法，取堆顶的元素作为下查找节点
+    tmpNode = openSet.top();
+
+    #ifdef SearchingVisulize
+    publishSearchNodes(*tmpNode, pub, pathNodes,counter);
+    #endif
+
+    openSet.pop();      //出栈
+    if ( reachGoal(tmpNode, goalPose) ) {
+      #ifdef debug_mode
+      ros::Time t1 = ros::Time::now();
+      ros::Duration d(t1 - t0);
+      std::cout << "got plan in ms: " << d * 1000 << std::endl;
+      #endif
+      ROS_INFO("Got a plan,loop %d times", counter);
+      nodeToPlan(tmpNode, plan);
+      return true;
+    }
+    else {
+      if (Constants::dubinsShot && tmpNode->isInRange(*goalPose) && !tmpNode->isReverse()) {
+        nSucc = dubinsShot(*tmpNode, *goalPose, costmap);
+        //如果Dubins方法能直接命中，即不需要进入Hybrid A*搜索了，直接返回结果
+        if (nSucc != nullptr && reachGoal(nSucc, goalPose) ) {
+          #ifdef debug_mode
+          ros::Time t1 = ros::Time::now();
+          ros::Duration d(t1 - t0);
+          std::cout << "got plan in ms: " << d * 1000 << std::endl;
+          #endif
+          ROS_INFO("Got a plan,expored %d nodes ", counter);
+          nodeToPlan(nSucc, plan);
+          return true;//如果下一步是目标点，可以返回了
+        }
+      } 
+      else if(Constants::reedsSheppShot && tmpNode->isInRange(*goalPose) && !tmpNode->isReverse()) {
+        nSucc = reedsSheppShot(*tmpNode, *goalPose, costmap);
+        //如果Dubins方法能直接命中，即不需要进入Hybrid A*搜索了，直接返回结果
+        if (nSucc != nullptr && reachGoal(nSucc, goalPose) ) {
+          #ifdef debug_mode
+          ros::Time t1 = ros::Time::now();
+          ros::Duration d(t1 - t0);
+          std::cout << "got plan in ms: " << d * 1000 << std::endl;
+          #endif
+          ROS_INFO("Got a plan,expored %d nodes ", counter);
+          nodeToPlan(nSucc, plan);
+          return true;//如果下一步是目标点，可以返回了
+        }
+      }
+    }
+    // 拓展tmpNode临时点目标周围的点，并且使用STL标准库的向量链表进行存储拓展点Node3D的指针数据
+    std::vector<Node3D*> adjacentNodes = gatAdjacentPoints(dir, cellsX, cellsY, charMap, tmpNode);   
+    // 将 tmpNode点在pathNode3D中映射的点加入闭集合中
+    closed_set.emplace(tmpNode->getindex(cells_x ,Constants::headings, resolution, dx, dy), tmpNode);
+    for (std::vector<Node3D*>::iterator it = adjacentNodes.begin(); it != adjacentNodes.end(); ++it) {
+      // 使用stl标准库中的interator迭代器遍历相邻点
+      Node3D* point = *it;
+      iPred = point->getindex(cells_x, Constants::headings, resolution, dx, dy);
+      if(closed_set.find(iPred)!= closed_set.end()) {
+          continue;
+      }
+      g = point->calcG();
+      if(open_set.find(iPred)!= open_set.end()) {
+        if(g < open_set[iPred]->getG()) {
+          point->setG(g);
+          open_set[iPred]->setG(g);
+          openSet.push(point);    // 如果符合拓展点要求，则将此点加入优先队列中  
+        }
+      } else {
+        point->setG(g);
+        open_set.emplace(iPred, point);
+        costmap->worldToMap(point->getX(), point->getY(), start_x, start_y);
+        updateH(*point, *goalPose, NULL, NULL, cells_x, cells_y, dp_map[start_y * cellsX + start_x]->getG()/20);
+        openSet.push(point);    // 如果符合拓展点要求，则将此点加入优先队列中 
+      }
+        
+    }
+  }
+  return false;
+}
+
+std::vector<Node3D*> hybridAstar::gatAdjacentPoints(int dir, int cells_x, 
+    int cells_y, const unsigned char* charMap, Node3D *point) {
+  std::vector<Node3D*> adjacentNodes;
+  Node3D *tmpPtr;
+  float xSucc;
+  float ySucc;
+  float tSucc;
+  unsigned int startX, startY;
+  float t = point->getT();
+  // int index;
+  float x = point->getX();
+  float y = point->getY();
+  unsigned int u32_x = int(x);
+  unsigned int u32_y = int(y);
+  for (int i = 0; i < dir; i++) {
+    if (i < 3) {
+      xSucc = x + Constants::dx[i] * cos(t) - Constants::dy[i] * sin(t);
+      ySucc = y + Constants::dx[i] * sin(t) + Constants::dy[i] * cos(t);
+    } else {
+      xSucc = x - Constants::dx[i - 3] * cos(t) - Constants::dy[i - 3] * sin(t);
+      ySucc = y - Constants::dx[i - 3] * sin(t) + Constants::dy[i - 3] * cos(t);
+    }
+    if (costmap->worldToMap(xSucc, ySucc, startX, startY)) {
+
+      if (charMap[startX + startY * cells_x] < 250) {
+        if (i < 3) {
+          tmpPtr = new Node3D(xSucc, ySucc, t + Constants::dt[i], 999, 0, false, point);
+          tmpPtr->setCost(charMap[startX + startY * cells_x]);
+        } else {
+          tmpPtr = new Node3D(xSucc, ySucc, t - Constants::dt[i - 3], 999, 0, true, point);
+          tmpPtr->setCost(charMap[startX + startY * cells_x]);
+        }
+        adjacentNodes.push_back(tmpPtr);
+      }
+    }
+  }
+
+  return adjacentNodes;
+}
+
+bool hybridAstar::reachGoal(Node3D* node, Node3D* goalPose) {
+  float nodeX = node->getX();
+  float nodeY = node->getY();
+  float goalX = goalPose->getX();
+  float goalY = goalPose->getY();
+  if ((nodeX < (goalX + point_accuracy) && nodeX > (goalX - point_accuracy)) && 
+      (nodeY < (goalY + point_accuracy) && nodeY > (goalY - point_accuracy)) ) {
+    if (node->getT()  < (goalPose->getT()+theta_accuracy )&& 
+        node->getT()  > (goalPose->getT()-theta_accuracy )) {
+      return true;
+    }
+  }
+  return false;
+}
+
+int hybridAstar::calcIndix(float x, float y, int cells_x, float t) {
+    return (int(x) * cells_x + int(y)) * Constants::headings + int(t / Constants::headings);
+}
+
+void hybridAstar::nodeToPlan(Node3D* node, std::vector<geometry_msgs::PoseStamped>& plan) {
+  Node3D* tmpPtr = node;
+  geometry_msgs::PoseStamped tmpPose;
+  std::vector<geometry_msgs::PoseStamped> replan;
+  // float resolution = costmap->getResolution();
+  unsigned int originalX,originalY;
+  tmpPose.header.stamp = ros::Time::now();   
+  //参数后期处理，发布到RViz上进行可视化
+  while(tmpPtr!=nullptr) {
+    tmpPose.pose.position.x = tmpPtr->getX();
+    tmpPose.pose.position.y = tmpPtr->getY();
+    tmpPose.header.frame_id = frame_id_;
+    tmpPose.pose.orientation = tf::createQuaternionMsgFromYaw(tmpPtr->getT());
+    replan.push_back(tmpPose);
+    tmpPtr = tmpPtr->getPerd();
+  }
+  int size = replan.size();
+  for (int i = 0;i < size; ++i) {
+    plan.push_back(replan[size - i -1 ]);
+  }
+
+}
+
+}//end of namespace hybrid_astar_planner

Src/hybrid_astar_planner-main/src/node2d.cpp

@@ -0,0 +1,133 @@
+/*********************************************************************
+ *
+ *  BSD 3-Clause License
+ *
+ *  Copyright (c) 2021, dengpw
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   1 Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   2 Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   3 Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived from
+ *     this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ * 
+ *  Author:  dengpw
+ *********************************************************************/
+#include "node2d.h"
+#include <iostream>
+namespace hybrid_astar_planner {
+
+float Node2D::calcG(Node2D const *partent) {
+  float g;
+  if((abs(x - partent->x) + abs(y - partent->y)) == 2) {
+    g = 1.4142;  
+  }
+  else g = 1;
+  return g + partent->getG() + 0.1*cost;
+}
+
+float Node2D::calcH(Node2D const *goal) {
+  float dx, dy;
+  dx = abs(x - goal->x);
+  dy = abs(y - goal->y);
+  h = dx + dy;
+}
+
+std::unordered_map<int, std::shared_ptr<Node2D>> GridSearch::GenerateDpMap(
+    const double goal_x, const double goal_y, 
+    costmap_2d::Costmap2D* costmap) {
+  const unsigned char* charMap = costmap->getCharMap(); 
+  int counter = 0;
+  int cells_x, cells_y;
+  cells_x = costmap->getSizeInCellsX();
+  cells_y = costmap->getSizeInCellsY();
+  float resolution = costmap->getResolution();
+  std::priority_queue<std::pair<int, double>,
+                      std::vector<std::pair<int, double>>, cmp> 
+    open_pq;
+  std::unordered_map<int, std::shared_ptr<Node2D>> open_set;
+  std::unordered_map<int, std::shared_ptr<Node2D>> dp_map_;
+  std::shared_ptr<Node2D> goal_node = std::make_shared<Node2D>(goal_x, goal_y);
+  goal_node->setX( goal_x );
+  goal_node->setY( goal_y );
+  goal_node->setG(0);
+  open_set.emplace(goal_node->getindex(cells_x), goal_node );
+  open_pq.emplace(goal_node->getindex(cells_x), goal_node->getG());
+  while(!open_pq.empty()) {
+    ++counter;
+    int id = open_pq.top().first;
+    open_pq.pop();
+    std::shared_ptr<Node2D> current_node = open_set[id];
+    dp_map_.emplace(current_node->getindex(cells_x), current_node);
+    std::vector<std::shared_ptr<Node2D>> adjacent_nodes = 
+        getAdjacentPoints(cells_x, cells_y, charMap, current_node );
+    for (std::vector<std::shared_ptr<Node2D>>::iterator 
+            it = adjacent_nodes.begin(); it != adjacent_nodes.end(); ++it) {
+      std::shared_ptr<Node2D> next_node = *it;
+      if (dp_map_.find(next_node->getindex(cells_x)) != dp_map_.end()) {
+        continue;
+      }
+      if (open_set.find(next_node->getindex(cells_x)) != open_set.end()) {
+        if (open_set[next_node->getindex(cells_x)]->getG() > next_node->getG()) {
+            open_set[next_node->getindex(cells_x)]->setCost(next_node->getG());
+            open_set[next_node->getindex(cells_x)]->setPerd_(current_node);
+        }
+      } else {
+          // ++counter;
+        next_node->setPerd_(current_node);
+        open_set.emplace(next_node->getindex(cells_x), next_node );
+        open_pq.emplace(next_node->getindex(cells_x), next_node->getG());
+      }
+    }
+  }
+  // printf("X %d y %d  explored node num is : %d \n", cells_x, cells_y, counter);
+  return dp_map_;
+}
+
+std::vector<std::shared_ptr<Node2D>> GridSearch::getAdjacentPoints(int cells_x,
+    int cells_y, const unsigned char* charMap, std::shared_ptr<Node2D> point) {
+  std::vector<std::shared_ptr<Node2D>> adjacentNodes;
+  float g = 0;
+  // std::cout << "the cost-so-far of this node" << point->getG() << std::endl;
+  for (int x = point->getX() - 1; x <= point->getX() + 1; ++x) {
+    for (int y = point->getY() - 1; y <= point->getY() + 1; ++y) {
+      if (charMap[x  + y* cells_x] < 250 &&x < cells_x && y < cells_y 
+          && x > 0 && y > 0) {
+        std::shared_ptr<Node2D> node = std::make_shared<Node2D>(x,y);
+        if((abs(x - point->getX()) + abs(y - point->getY())) == 2) {
+          g = 1.4142;
+        } else {
+          g = 1;
+        }
+        node->SetPathCost(point->getG() + 0.1*charMap[x  + y* cells_x] + g);
+        node->setX(x);
+        node->setY(y);
+        adjacentNodes.emplace_back(node);
+      }
+    }
+  }//end of for
+  return adjacentNodes;
+}
+
+}//namespace hybrid_astar_planner

Src/hybrid_astar_planner-main/src/node3d.cpp

@@ -0,0 +1,109 @@
+/*********************************************************************
+ *
+ *  BSD 3-Clause License
+ *
+ *  Copyright (c) 2021, dengpw
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   1 Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   2 Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   3 Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived from
+ *     this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ * 
+ *  Author:  dengpw
+ *********************************************************************/
+#include "node3d.h"
+#include <iostream>
+#include <math.h>
+// 设计启发函数
+namespace hybrid_astar_planner {
+
+void Node3D::setT(float _t) {
+  if (_t>3.1415) {
+    _t = _t - 6.283;
+  }
+  t = _t / Constants::deltaHeadingRad;
+}
+
+
+float Node3D::calcG() {
+  float g;
+  if(reverse) {
+    // 如果进行的是倒车，对倒车、转向、改变方向进行惩罚
+    if (reverse != perd->reverse) {                     // 对改变行驶方向进行惩罚
+      g = Constants::dx[0] * Constants::penaltyCOD * Constants::penaltyReversing;
+    }
+    else {
+      g = Constants::dx[0] * Constants::penaltyReversing; 
+    }
+  }
+  else {
+    // 如果此时位车辆前进情况，对其进行响应的代价计算
+    if (reverse != perd->reverse) {                     //对方向改变进行的惩罚
+      g = Constants::dx[0] * Constants::penaltyCOD;
+    }
+    else {
+      if (t == perd->t) {
+        g = Constants::dx[0];
+      }
+      else {
+        g = Constants::dx[0] * Constants::penaltyTurning;
+      }
+    }
+  }
+  return g + perd->getG();
+}
+
+float Node3D::calcH(Node3D const *goal) {
+  float dx, dy;
+  dx = fabs(x - goal->x);
+  dy = fabs(y - goal->y);
+  h = dx +dy;
+}
+
+
+
+//###################################################
+//                                        IS IN RANGE
+//###################################################
+bool Node3D::isInRange(const Node3D& goal) const {
+  int random = rand() % 10 + 1;//产生位于[1, 10]的随机数
+  float dx = std::abs(x - goal.x) / random;
+  float dy = std::abs(y - goal.y) / random;
+  return (dx * dx) + (dy * dy) < Constants::dubinsShotDistance;//距离的平方和在100以内则认为可达
+}
+
+//###################################################
+//                                 3D NODE COMPARISON
+//###################################################
+//3d节点的比较函数：x和y同时相同、并且theta在一个阈值范围内时可以认为是同一个Node
+bool Node3D::operator == (const Node3D& rhs) const {
+  return (int)x == (int)rhs.x &&
+          (int)y == (int)rhs.y &&
+          (std::abs(t - rhs.t) <= Constants::deltaHeadingRad ||
+          std::abs(t - rhs.t) >= Constants::deltaHeadingNegRad);
+}
+
+} // namespace hybrid_astar_planner

Src/hybrid_astar_planner-main/src/planner_core.cpp

@@ -0,0 +1,156 @@
+/*********************************************************************
+ *
+ *  BSD 3-Clause License
+ *
+ *  Copyright (c) 2021, dengpw
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   1 Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   2 Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   3 Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived from
+ *     this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ * 
+ *  Author:  dengpw
+ *********************************************************************/
+#include <iostream>
+#include "planner_core.h"
+#include <tf/transform_datatypes.h>
+#include <ros/node_handle.h>
+#include "astar.h"
+#include "hybrid_astar.h"
+
+PLUGINLIB_EXPORT_CLASS(hybrid_astar_planner::HybridAStarPlanner, nav_core::BaseGlobalPlanner)//注册为类插件的声明
+
+namespace hybrid_astar_planner {
+
+HybridAStarPlanner::HybridAStarPlanner():
+    initialized_(false),costmap(NULL),resolution(1.0) {
+  std::cout << "creating the hybrid Astar planner" << std::endl;
+}
+
+
+void HybridAStarPlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
+{
+  initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());
+}
+
+
+void HybridAStarPlanner::initialize(std::string name, costmap_2d::Costmap2D *_costmap, std::string frame_id) {
+  if(!initialized_) {
+    ROS_INFO("initializing the hybrid Astar planner");
+    // 订阅global_costmap的内容，以便获取参数
+    ros::NodeHandle nh("~/global_costmap");
+    ros::NodeHandle nh2("~/");
+    ros::NodeHandle private_nh("~/" + name);
+    nh2.param("use_hybrid_astar", use_hybrid_astar, true);
+    if(use_hybrid_astar) {
+      ROS_INFO("Using hybrid_astar mode!");
+    } else {
+      ROS_INFO("Using Astar mode!");
+    }
+    costmap = _costmap;
+    frame_id_ = frame_id;
+    std::cout << frame_id << std::endl;
+    //  初始化发布路径的主题
+    plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
+    path_vehicles_pub_ = private_nh.advertise<visualization_msgs::MarkerArray>("pathVehicle", 1);
+    make_plan_srv_ = private_nh.advertiseService("make_plan", &HybridAStarPlanner::makePlanService, this);
+  }
+  initialized_ = true;
+}//end of constructor function HybridAStarPlanner
+
+HybridAStarPlanner::~HybridAStarPlanner() {
+
+}//end of deconstructor function HybridAStarPlanner
+
+
+bool HybridAStarPlanner::makePlanService(nav_msgs::GetPlan::Request& req, 
+    nav_msgs::GetPlan::Response& resp) {
+  makePlan(req.start, req.goal, resp.plan.poses);
+  resp.plan.header.stamp = ros::Time::now();
+  resp.plan.header.frame_id = frame_id_;
+  return true;
+}
+
+
+bool HybridAStarPlanner::makePlan(const geometry_msgs::PoseStamped &start,
+    const geometry_msgs::PoseStamped &goal, 
+    std::vector<geometry_msgs::PoseStamped>& plan) {
+  // std::cout << "the start pose of planner x:" << start.pose.position.x << " y:" << start.pose.position.y << std::endl;
+  // std::cout << "the goal pose of planner x:" << goal.pose.position.x << " y:" << goal.pose.position.y << std::endl;
+  Expander* _planner;
+
+  // ROS_INFO("the resolution of cost map: %f ",costmap->getResolution());
+  if (use_hybrid_astar) {  
+    _planner = new hybridAstar(frame_id_,costmap);
+  }
+  else {
+    _planner = new astar(frame_id_,costmap);
+  }
+
+  //检查设定的目标点参数是否合规
+  if(!(checkStartPose(start) && checkgoalPose(goal))) {
+    ROS_WARN("Failed to create a global plan!");
+    return false;
+  }
+  plan.clear();
+  //正式将参数传入规划器中
+  if(!_planner->calculatePath(start, goal , costmap->getSizeInCellsX(), costmap->getSizeInCellsY(), plan, path_vehicles_pub_, pathNodes)) {
+    return false;
+  }
+  
+  //参数后期处理，发布到RViz上进行可视化
+  clearPathNodes();
+  //path只能发布2D的节点
+  publishPlan(plan);
+  publishPathNodes(plan);
+  return true;
+}//end of makeplan
+
+bool HybridAStarPlanner::checkStartPose(const geometry_msgs::PoseStamped &start) {
+    unsigned int startx,starty;
+  if (costmap->worldToMap(start.pose.position.x, start.pose.position.y, startx, starty)) {
+    return true;
+  }
+  ROS_WARN("The Start pose is out of the map!");
+  return false;
+}//end of checkStartPose
+
+bool HybridAStarPlanner::checkgoalPose(const geometry_msgs::PoseStamped &goal) {
+  unsigned int goalx,goaly;
+  if (costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, goalx, goaly)) {
+    if (costmap->getCost( goalx, goaly ) > 252) {
+      // std::cout << costmap->getCost(goalx, goaly) << std::endl;
+      ROS_WARN("The Goal pose is out of the map! %d",costmap->getCost(goalx, goaly));
+      ROS_WARN("The Goal pose is occupied , please reset the goal!");
+      return false;
+    }
+    return true;
+  }
+  return false;
+}//end of checkgoalPose
+
+}//namespace hybrid_astar_planner
+

Src/hybrid_astar_planner-main/src/visualize.cpp

@@ -0,0 +1,139 @@
+/*********************************************************************
+ *
+ *  BSD 3-Clause License
+ *
+ *  Copyright (c) 2021, dengpw
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   1 Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   2 Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   3 Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived from
+ *     this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ * 
+ *  Author:  dengpw
+ *********************************************************************/
+#include "planner_core.h"
+#include "node3d.h"
+#include <tf/transform_datatypes.h>
+namespace hybrid_astar_planner {
+  
+void HybridAStarPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path) {
+  if (!initialized_) {
+    ROS_ERROR(
+            "This planner has not been initialized yet, but it is being used, please call initialize() before use");
+    return;
+  }
+  //create a message for the plan
+  geometry_msgs::PoseStamped transform_path;
+  nav_msgs::Path gui_path;
+  int size = path.size();
+  gui_path.poses.resize(size);
+
+  gui_path.header.frame_id = frame_id_;
+  gui_path.header.stamp = ros::Time::now();
+
+  // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+  for (unsigned int i = 0; i < size; i++) {
+    transform_path.pose.position = path[i].pose.position;
+    gui_path.poses[i] = transform_path;//
+  }
+
+  plan_pub_.publish(gui_path);
+  // ROS_INFO("Publish the path to Rviz");
+    
+}//end of publishPlan
+
+
+void HybridAStarPlanner::publishPathNodes(const std::vector<geometry_msgs::PoseStamped>& path) {
+  if (!initialized_) {
+    ROS_ERROR(
+            "This planner has not been initialized yet, but it is being used, please call initialize() before use");
+    return;
+  }
+  visualization_msgs::Marker pathVehicle;
+  int nodeSize = path.size();
+  pathVehicle.header.stamp = ros::Time(0);
+  pathVehicle.color.r = 52.f / 255.f;
+  pathVehicle.color.g = 250.f / 255.f;
+  pathVehicle.color.b = 52.f / 255.f;
+  pathVehicle.type = visualization_msgs::Marker::ARROW;
+  pathVehicle.header.frame_id = frame_id_;
+  pathVehicle.scale.x = 0.22;
+  pathVehicle.scale.y = 0.18;
+  pathVehicle.scale.z = 0.12;
+  pathVehicle.color.a = 0.1;
+  // 转化节点，并同时加上时间戳等信息
+  for(int i = 0; i<nodeSize; i++) {
+    pathVehicle.header.stamp = ros::Time(0);
+    pathVehicle.pose = path[i].pose;
+    pathVehicle.id = i;
+    pathNodes.markers.push_back(pathVehicle);
+  }
+  // 发布这些车辆位置标记点
+  path_vehicles_pub_.publish(pathNodes);
+    
+}//end of publishPathNodes
+
+void HybridAStarPlanner::clearPathNodes() {
+  // 初始化并配置节点为全清空模式
+  visualization_msgs::Marker node;
+  pathNodes.markers.clear();
+  node.action = visualization_msgs::Marker::DELETEALL;
+  node.header.frame_id = frame_id_;
+  node.header.stamp = ros::Time(0);
+  node.id = 0;
+  node.action = 3;
+  pathNodes.markers.push_back(node);
+  path_vehicles_pub_.publish(pathNodes);
+  // ROS_INFO("Clean the path nodes");
+}
+
+ void publishSearchNodes(Node3D node,ros::Publisher& pub, 
+    visualization_msgs::MarkerArray& pathNodes, int i) {
+  visualization_msgs::Marker pathVehicle;
+  pathVehicle.header.stamp = ros::Time(0);
+  pathVehicle.color.r = 250.f / 255.f;
+  pathVehicle.color.g = 250.f / 255.f;
+  pathVehicle.color.b = 52.f / 255.f;
+  pathVehicle.type = visualization_msgs::Marker::ARROW;
+  pathVehicle.header.frame_id = "map";
+  pathVehicle.scale.x = 0.22;
+  pathVehicle.scale.y = 0.18;
+  pathVehicle.scale.z = 0.12;
+  pathVehicle.color.a = 0.1;
+  // 转化节点，并同时加上时间戳等信息
+  pathVehicle.header.stamp = ros::Time(0);
+  pathVehicle.pose.position.x = node.getX();
+  pathVehicle.pose.position.y = node.getY();
+  pathVehicle.pose.position.z = 0;
+  pathVehicle.pose.orientation = tf::createQuaternionMsgFromYaw(node.getT());
+  pathVehicle.id = i;
+  pathNodes.markers.push_back(pathVehicle);
+  
+  // 发布这些车辆位置标记点
+  pub.publish(pathNodes);
+    
+}//end of publishPathNodes   
+}

Src/hybrid_astar_planner-main/test_the_plugin/include/test_plugins.h

@@ -0,0 +1,82 @@
+#ifndef _TEST_PLUGINS
+#define _TEST_PLUGINS
+/*********************************************************************
+ *
+ *  BSD 3-Clause License
+ *
+ *  Copyright (c) 2021, dengpw
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   1 Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   2 Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   3 Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived from
+ *     this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <nav_core/base_global_planner.h>
+#include <tf2_ros/transform_listener.h>
+#include <boost/shared_ptr.hpp>
+#include <ros/node_handle.h>
+class TestPlanner{
+    public:
+        /**
+         * @brief   Default constructor for the TestPlanner
+         * @param tf A reference to a TransformListener
+         */
+        TestPlanner(tf2_ros::Buffer &tf);
+        /**
+         * @brief   Destructor - Cleans up 
+         **/
+        ~TestPlanner();
+        /**
+         * @brief   Call back function of a suscriber 目的是接收RViz传过来的goal pose
+         * @param  _goal A reference to geometry_msgs::PoseStamped::ConstPtr
+         */
+        void setgoal(const geometry_msgs::PoseStamped::ConstPtr& _goal);
+
+    private:
+        /**
+         * @brief   Transform the Start pose from tf tree 将起始点从TF转化树中提取出来(测试程序将bese_link作为起始点提取)
+         * 
+         */
+        bool transformStarPose(void);
+        //智能指针，用来加载类插件，保存类插件的地址。这里没有使用标准库是因为Classloder的createInstance函数支持boost库，并没有使用标准库
+        boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
+        //路径规划的vector类模板，用来保存路径
+        std::vector<geometry_msgs::PoseStamped>* planner_plan_;
+        geometry_msgs::PoseStamped goal_pose;
+        geometry_msgs::PoseStamped start_pose;
+        geometry_msgs::TransformStamped start_transform;
+        ros::NodeHandle n;
+        costmap_2d::Costmap2DROS* costmap;
+        geometry_msgs::PoseStamped robot_pose;
+        std::string global_planner;
+        tf2_ros::Buffer& tf;
+        ros::Subscriber make_plane;
+
+};
+
+#endif //!_TEST_PLUGINS

Src/hybrid_astar_planner-main/test_the_plugin/launch/config.rviz

@@ -0,0 +1,221 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Map2
+        - /Path1
+        - /MarkerArray3
+      Splitter Ratio: 0.5
+    Tree Height: 300
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /move_base_simple/goal
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /move_base_simple/start
+      Unreliable: false
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /sPathNodes
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /sPathVehicle
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.10000000149011612
+      Reference Frame: base_link
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /move_base/global_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /move_base/HybridAStarPlanner/plan
+      Unreliable: false
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /move_base/HybridAStarPlanner/pathVehicle
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Angle: 0.019999396055936813
+      Class: rviz/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 78.33514404296875
+      Target Frame: <Fixed Frame>
+      Value: TopDownOrtho (rviz)
+      X: 2.5319337844848633
+      Y: 1.5114061832427979
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 597
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000001b7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000001b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000001b7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004e40000003efc0100000002fb0000000800540069006d00650100000000000004e4000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000273000001b700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1252
+  X: 317
+  Y: 27

Src/hybrid_astar_planner-main/test_the_plugin/launch/test.launch

@@ -0,0 +1,18 @@
+<launch>
+    <node name="move_base" pkg="hybrid_astar_planner" type="test_planner" output="screen">
+    <!-- <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"> -->
+        <rosparam file="$(find hybrid_astar_planner)/test_the_plugin/param/costmap_common_params.yaml" command="load" ns="global_costmap" /> 
+        <rosparam file="$(find hybrid_astar_planner)/test_the_plugin/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
+        <rosparam file="$(find hybrid_astar_planner)/test_the_plugin/param/local_costmap_params.yaml" command="load" />
+        <rosparam file="$(find hybrid_astar_planner)/test_the_plugin/param/global_costmap_params.yaml" command="load" /> 
+        <rosparam file="$(find hybrid_astar_planner)/test_the_plugin/param/base_local_planner_params.yaml" command="load" />
+        <rosparam file="$(find hybrid_astar_planner)/test_the_plugin/param/move_base_params.yaml" command="load" />
+        <rosparam file="$(find hybrid_astar_planner)/test_the_plugin/param/plugins.yaml" command="load" ns="/costmap_node/costmap"/>
+        <param name="base_global_planner" value="global_planner/GlobalPlanner" /><!--这个标志是选择使用特定的全局规划器-->
+        <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
+        <param name="use_hybrid_astar" value="true" />
+    </node>
+    <node name="tf_broadcaster" pkg="hybrid_astar_planner" type="tf_test_broadcaster" output="screen" />
+    <node name="map_server" pkg="map_server" type="map_server" args="$(find hybrid_astar_planner)/test_the_plugin/maps/mymap2077.yaml" />
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find hybrid_astar_planner)/test_the_plugin/launch/config.rviz" />
+</launch>

Src/hybrid_astar_planner-main/test_the_plugin/maps/map.yaml

@@ -0,0 +1,8 @@
+image: map_demo.png
+resolution: 1
+origin: [0.0, 0.0, 0.0]
+occupied_thresh: 0.1
+free_thresh: 0.05
+negate: 0
+
+

Src/hybrid_astar_planner-main/test_the_plugin/maps/mm.yaml

@@ -0,0 +1,7 @@
+image: mm.pgm
+resolution: 0.050000
+origin: [-1.000000, -12.200000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap.yaml

@@ -0,0 +1,7 @@
+image: mymap.pgm
+resolution: 0.050000
+origin: [-50.000000, -50.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap2077.yaml

@@ -0,0 +1,7 @@
+image: mymap2077.pgm
+resolution: 0.050000
+origin: [-7.000000, -7.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap2088.yaml

@@ -0,0 +1,7 @@
+image: mymap2088.pgm
+resolution: 0.050000
+origin: [-7.000000, -7.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap4.yaml

@@ -0,0 +1,7 @@
+image: mymap4.pgm
+resolution: 0.050000
+origin: [-50.000000, -50.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap5.yaml

@@ -0,0 +1,7 @@
+image: mymap5.pgm
+resolution: 0.050000
+origin: [-12.200000, -1.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

Src/hybrid_astar_planner-main/test_the_plugin/param/base_local_planner_params.yaml

@@ -0,0 +1,130 @@
+# TrajectoryPlannerROS:
+
+#   meter_scoring: true
+#   holonomic_robot: false
+
+#   max_vel_x: 0.6
+#   min_vel_x: 0.1
+
+#   max_vel_y: 0
+#   min_vel_y: 0
+
+#   max_vel_theta: 1.2
+#   min_vel_theta: -1.2
+
+#   min_in_place_vel_theta: 0.5
+#   escape_vel: -0.5
+
+#   acc_lim_theta: 1
+#   acc_lim_x: 0.1
+#   acc_lim_y: 0
+# xy_goal_tolerance: 0.15
+# yaw_goal_tolerance: 0.15
+TebLocalPlannerROS:
+
+  odom_topic: odom
+  map_frame: /odom
+      
+  # Trajecto
+
+  teb_autosize: True
+  dt_ref: 0.1 #期望的轨迹时间分辨率
+  dt_hysteresis: 0.01 #根据当前时间分辨率自动调整大小的滞后现象，通常约为。建议使用dt ref的10%
+  
+  global_plan_overwrite_orientation: True #覆盖由全局规划器提供的局部子目标的方向
+  max_global_plan_lookahead_dist: 3.0 #指定考虑优化的全局计划子集的最大长度
+  feasibility_check_no_poses: 4 #每个采样间隔的姿态可行性分析数，default：4
+      
+
+  allow_init_with_backwards_motion: True
+  global_plan_viapoint_sep: -1
+  global_plan_prune_distance: 1
+  exact_arc_length: False
+  publish_feedback: False
+
+  # Robot
+  max_vel_x: 5
+  max_vel_x_backwards: 2
+  max_vel_theta: 1
+  acc_lim_x: 3
+  acc_lim_theta: 1
+
+  #仅适用于全向轮
+  # max_vel_y (double, default: 0.0)  
+  # acc_lim_y (double, default: 0.5)
+
+  # ********************** Carlike robot parameters ********************
+  min_turning_radius: 1       # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
+  wheelbase: 0.325                 # Wheelbase of our robot
+  cmd_angle_instead_rotvel: True # stage simulator takes the angle instead of the rotvel as input (twist message)
+  # ********************************************************************
+
+  footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
+    type: "line"
+    # radius: 0.2 # for type "circular"
+    line_start: [-0.2, 0.0] # for type "line"
+    line_end: [0.2, 0.0] # for type "line"
+    # front_offset: 0.2 # for type "two_circles"
+    # front_radius: 0.2 # for type "two_circles"
+    # rear_offset: 0.2 # for type "two_circles"
+    # rear_radius: 0.2 # for type "two_circles"
+    # vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"
+
+  # GoalTolerance
+      
+  xy_goal_tolerance: 0.2
+  yaw_goal_tolerance: 0.1
+  free_goal_vel: False
+  # complete_global_plan: True   
+  # Obstacles
+      
+  min_obstacle_dist: 0.1 #0.3 # 与障碍的最小期望距离,注意，teb_local_planner本身不考虑膨胀半径
+  include_costmap_obstacles: True #应否考虑到局部costmap的障碍
+  costmap_obstacles_behind_robot_dist: 0.3 #考虑后面n米内的障碍物
+  obstacle_poses_affected: 30 #为了保持距离，每个障碍物位置都与轨道上最近的位置相连。
+  costmap_converter_plugin: ""
+  costmap_converter_spin_thread: True
+  costmap_converter_rate: 5
+
+  # Optimization
+      
+  no_inner_iterations: 5
+  no_outer_iterations: 4
+  optimization_activate: True
+  optimization_verbose: False
+  penalty_epsilon: 0.1
+  weight_max_vel_x: 2
+  weight_max_vel_theta: 1
+  weight_acc_lim_x: 1
+  weight_acc_lim_theta: 1
+  weight_kinematics_nh: 1000
+  weight_kinematics_forward_drive: 1
+  weight_kinematics_turning_radius: 1
+  weight_optimaltime: 1
+  weight_obstacle: 50
+  weight_dynamic_obstacle: 10 # not in use yet
+  alternative_time_cost: False # not in use yet
+  selection_alternative_time_cost: False
+  # Homotopy Class Planner
+
+  enable_homotopy_class_planning: False
+  enable_multithreading: False
+  simple_exploration: False
+  max_number_classes: 4
+  roadmap_graph_no_samples: 15
+  roadmap_graph_area_width: 5
+  h_signature_prescaler: 0.5
+  h_signature_threshold: 0.1
+  obstacle_keypoint_offset: 0.1
+  obstacle_heading_threshold: 0.45
+  visualize_hc_graph: False
+
+  # # Recovery
+
+  # shrink_horizon_backup: True
+  # shrink_horizon_min_duration: 10
+  # oscillation_recovery: True
+  # oscillation_v_eps: 0.1
+  # oscillation_omega_eps: 0.1
+  # oscillation_recovery_min_duration: 10
+  # oscillation_filter_duration: 10

Src/hybrid_astar_planner-main/test_the_plugin/param/costmap_common_params.yaml

@@ -0,0 +1,22 @@
+obstacle_range: 2.5
+raytrace_range: 3.5
+# footprint: [[-0.08, -0.15], [-0.08, 0.15],[0.42, 0.15], [0.42, -0.15]]
+robot_radius: 0.1
+inflation_radius: 0.5
+transform_tolerance: 0.6
+observation_sources: scan
+# obstacles:
+#    observation_sources: scan
+#    scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}
+map: 
+  data_type: LaserScan 
+  topic: /map 
+  marking: true 
+  clearing: true
+scan: 
+  data_type: LaserScan 
+  topic: scan 
+  marking: true 
+  clearing: true
+
+map_type: costmap

Src/hybrid_astar_planner-main/test_the_plugin/param/costmap_common_params_mark.txt

@@ -0,0 +1,23 @@
+这些参数设置了代价图中的障碍物信息的阈值。
+obstacle_range: 2.5  #此参数确定了最大范围传感器读数，在此范围内的障碍物将会被标记并放入代价图中
+raytrace_range: 3.5  #这个范围内不存在的障碍物都会被清除
+# footprint: [[-0.08, -0.15], [-0.08, 0.15],[0.42, 0.15], [0.42, -0.15]]
+robot_radius: 0.15
+inflation_radius: 0.8
+transform_tolerance: 0.6
+observation_sources: scan
+# obstacles:
+#    observation_sources: scan
+#    scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}
+map: 
+  data_type: LaserScan 
+  topic: /map 
+  marking: true 
+  clearing: true
+scan: 
+  data_type: LaserScan 
+  topic: scan 
+  marking: true 
+  clearing: true
+
+map_type: costmap

Src/hybrid_astar_planner-main/test_the_plugin/param/global_costmap_params.yaml

@@ -0,0 +1,16 @@
+global_costmap:
+  global_frame: map
+  robot_base_frame: base_link
+  update_frequency: 1.0
+  publish_frequency: 0.5
+  static_map: true
+  transform_tolerance: 0.5
+  cost_scaling_factor: 10.0
+  # inflation_radius: 0.4  
+  resolution: 1.0
+  plugins:
+      - {name: static_map,       type: "costmap_2d::StaticLayer"}
+      - {name: obstacles,        type: "costmap_2d::VoxelLayer"}  
+      - {name: inflation_layer,        type: "costmap_2d::InflationLayer"}
+
+

Src/hybrid_astar_planner-main/test_the_plugin/param/local_costmap_params.yaml

@@ -0,0 +1,13 @@
+local_costmap:
+  global_frame: map
+  robot_base_frame: base_link
+  update_frequency: 5.0
+  publish_frequency: 2.0
+  static_map: false
+  rolling_window: true
+  width: 5
+  height: 5
+  resolution: 0.05
+  transform_tolerance: 0.5
+  cost_scaling_factor: 5
+  # inflation_radius: 0.3

Src/hybrid_astar_planner-main/test_the_plugin/param/move_base_params.yaml

@@ -0,0 +1,13 @@
+shutdown_costmaps: false
+
+controller_frequency: 5.0
+controller_patience: 3.0
+
+planner_frequency: 0.5
+planner_patience: 5.0
+
+oscillation_timeout: 10.0
+oscillation_distance: 0.2
+
+conservative_reset_dist: 0.1
+

Src/hybrid_astar_planner-main/test_the_plugin/param/plugins.yaml

@@ -0,0 +1,5 @@
+plugins: []  
+plugins:
+      - {name: static_map,       type: "costmap_2d::StaticLayer"}
+      - {name: obstacles,        type: "costmap_2d::VoxelLayer"}  
+      - {name: inflation_layer,        type: "costmap_2d::InflationLayer"}

Src/hybrid_astar_planner-main/test_the_plugin/src/test.cpp

@@ -0,0 +1,54 @@
+/**
+   @file  test.cpp
+   @brief Main entry point of the test program, the test program is using to test the hybrid_Astar plugin of
+          move_base package.
+   @author dengpw 2021/02/21 
+*/
+/*********************************************************************
+ *
+ *  BSD 3-Clause License
+ *
+ *  Copyright (c) 2021, dengpw
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   1 Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   2 Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   3 Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived from
+ *     this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+#include <tf/transform_listener.h>
+#include "test_plugins.h"
+int main(int argc, char** argv) {
+    ros::init(argc,argv,"plugin_test_node");
+    //ros::Duration(10)代表10秒的时间，在这里表示'tf2_ros::Buffer'实例化的对象保留10秒内的转换数据
+    tf2_ros::Buffer buffer(ros::Duration(10));                                 
+    //监听转换
+    tf2_ros::TransformListener tf(buffer);                                      
+    TestPlanner test( buffer );
+
+    ros::spin();
+    return 0;
+}

Src/hybrid_astar_planner-main/test_the_plugin/src/test_plugins.cpp

@@ -0,0 +1,106 @@
+/*********************************************************************
+ *
+ *  BSD 3-Clause License
+ *
+ *  Copyright (c) 2021, dengpw
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   1 Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   2 Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   3 Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived from
+ *     this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+#include <iostream>
+#include <tf/tf.h>
+#include <pluginlib/class_loader.h>
+#include <pluginlib_tutorials/polygon_base.h>
+#include "test_plugins.h"
+
+TestPlanner::TestPlanner(tf2_ros::Buffer &_tf):
+tf(_tf){
+    //订阅目标主题，绑定响应函数,这里使用suscribe订阅目标点，当目标点刷新就重新进行路径规划
+    make_plane = n.subscribe("/move_base_simple/goal", 1, &TestPlanner::setgoal, this);
+    //定义类插件的名称，以便之后接入系统
+    global_planner = std::string("hybrid_astar_planner/HybridAStarPlanner");    
+    // ros::NodeHandle nh("~/global_costmap");
+    pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"); //导入插件
+    planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
+    //这个global_costmap,是它自己的名字，这里需要用param服务器给到costmap的参数，才会初始化
+
+    // 需要注意的是，这里的初始化函数Costmap2DROS在构造时候的问题，~代表的是私有域的数据
+    costmap = new costmap_2d::Costmap2DROS("global_costmap", tf);         
+
+    std::cout << "creat the global costmap" << std::endl;
+    //指定costmap中的base_link为起始坐标
+    robot_pose.header.frame_id = "base_link";  
+                                     
+    transformStarPose();
+    try
+    {
+        planner_=bgp_loader_.createInstance(global_planner);                    
+        planner_->initialize(bgp_loader_.getName(global_planner),costmap);
+    }
+    catch (const pluginlib::PluginlibException &ex)
+    {
+      ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
+      exit(1);
+    }
+}
+void TestPlanner::setgoal(const geometry_msgs::PoseStamped::ConstPtr& _goal) {
+    // std::cout << "receved the goal pose" <<std::endl;
+    goal_pose.pose = _goal->pose;
+    goal_pose.pose.orientation = _goal->pose.orientation;
+    goal_pose.header = _goal->header;
+    transformStarPose();
+    costmap->start();
+    planner_->makePlan(start_pose, goal_pose, *planner_plan_);
+}
+
+bool TestPlanner::transformStarPose(void){
+    try
+    {
+        start_transform = tf.lookupTransform("map", "base_link", ros::Time(0), ros::Duration(3.0));
+    }
+    catch(const std::exception& e)
+    {
+        std::cerr << e.what() << '\n';
+        return false;
+    }
+    start_pose.pose.position.x = start_transform.transform.translation.x;
+    start_pose.pose.position.y = start_transform.transform.translation.y;
+    start_pose.pose.position.z = start_transform.transform.translation.z;
+    start_pose.pose.orientation.w = start_transform.transform.rotation.w;
+    start_pose.pose.orientation.x = start_transform.transform.rotation.x;
+    start_pose.pose.orientation.y = start_transform.transform.rotation.y;
+    start_pose.pose.orientation.z = start_transform.transform.rotation.z;
+    return true;
+}
+TestPlanner::~TestPlanner() {
+    planner_.reset();
+    if(costmap) {
+        delete costmap;
+    }
+}

Src/hybrid_astar_planner-main/test_the_plugin/src/tf_broadcaster.cpp

@@ -0,0 +1,84 @@
+/**
+ * @file tf_broadcaster.cpp
+ * @brief 这时实现了一个坐标系统转换的TF变换:
+ *  1) odom -> map 
+ *  2) map  -> path
+ *  3) map  -> base_link
+ */
+
+//###################################################
+//                        TF MODULE FOR THE HYBRID A*
+//                        用于测试混合A*算法的一个模组
+//###################################################
+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+// map pointer
+nav_msgs::OccupancyGridPtr grid;//定义广播信息的格式
+tf::Transform transform;        //创建tf的广播器
+tf::Quaternion q;
+
+// map callback
+void setMap(const nav_msgs::OccupancyGrid::Ptr map) {
+  std::cout << "Creating transform for map..." << std::endl;
+  grid = map;
+}
+
+void start_pose_callback(const geometry_msgs::PoseWithCovarianceStampedPtr& post) {
+  
+  // 初始化tf数据
+  transform.setOrigin(tf::Vector3(post->pose.pose.position.x, post->pose.pose.position.y, 0.0));
+  //定义旋转的四元参数
+  //具体的TF变换内容可以参考《机器人学导论》（美）克来格（Craig,J.J）
+  q.setX(post->pose.pose.orientation.x);
+  q.setY(post->pose.pose.orientation.y);
+  q.setZ(post->pose.pose.orientation.z);
+  q.setW(post->pose.pose.orientation.w);
+  transform.setRotation(q);
+  // 广播world与base_link之间的tf数据
+  
+  std::cout << "Transform initialpost to base_link" << std::endl;
+}
+
+int main(int argc, char** argv) {
+  // initiate the broadcaster
+  ros::init(argc, argv, "tf_broadcaster");
+  ros::NodeHandle n;
+  ros::NodeHandle nh;
+  // subscribe to map updates
+  std::cout << "Transform initialpost to base_link" << std::endl;
+  ros::Subscriber sub_map = n.subscribe("/occ_map", 1, setMap);
+  ros::Subscriber sub_start = nh.subscribe("/initialpose", 1, &start_pose_callback);
+  tf::Pose tfPose;
+  ros::Rate r(10);
+  tf::TransformBroadcaster broadcaster;
+  tf::TransformBroadcaster br;
+  transform.setOrigin(tf::Vector3(0, 0, 0.0));
+  q.setX(0);
+  q.setY(0);
+  q.setZ(0);
+  q.setW(1);
+  transform.setRotation(q);
+  while (ros::ok()) {
+    // transform from geometry msg to TF
+    if (grid != nullptr) {
+      tf::poseMsgToTF(grid->info.origin, tfPose);
+    }
+    // listener
+    // odom to map 从odom到地图坐标系
+    broadcaster.sendTransform(
+      tf::StampedTransform(
+        tf::Transform(tf::Quaternion(0, 0, 0, 1), tfPose.getOrigin()),
+        ros::Time::now(), "odom", "map"));
+    // map to path 从map到路径坐标系，这里的路径坐标系是三维的坐标系
+    broadcaster.sendTransform(
+      tf::StampedTransform(
+        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
+        ros::Time::now(), "map", "path"));
+    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "base_link"));
+    ros::spinOnce();
+    r.sleep();
+  }
+}