diff --git a/README.md b/README.md
deleted file mode 100644
index 846f0d84..00000000
--- a/README.md
+++ /dev/null
@@ -1,2 +0,0 @@
-# project
-
diff --git a/Src/hybrid_astar_planner-main/.vscode/c_cpp_properties.json b/Src/hybrid_astar_planner-main/.vscode/c_cpp_properties.json
new file mode 100644
index 00000000..2139473a
--- /dev/null
+++ b/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
+}
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/.vscode/settings.json b/Src/hybrid_astar_planner-main/.vscode/settings.json
new file mode 100644
index 00000000..5809a907
--- /dev/null
+++ b/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"
+ }
+}
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/CMakeLists.txt b/Src/hybrid_astar_planner-main/CMakeLists.txt
new file mode 100644
index 00000000..fe6e5296
--- /dev/null
+++ b/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}
+)
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/LICENSE b/Src/hybrid_astar_planner-main/LICENSE
new file mode 100644
index 00000000..5b18a23b
--- /dev/null
+++ b/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.
diff --git a/Src/hybrid_astar_planner-main/README.md b/Src/hybrid_astar_planner-main/README.md
new file mode 100644
index 00000000..3deffac1
--- /dev/null
+++ b/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
+
+### Dependencies
+Install all dependencies
+* [Open Motion Planning Library (OMPL)](http://ompl.kavrakilab.org/)
+* [ros_map_server](http://wiki.ros.org/map_server)
+
+### Setup
+
+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:
+ < `param name="base_global_planner" value="global_planner/GlobalPlanner"` / >
+ < `param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS"` / >
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/hybrid_astar_planner_plugin.xml b/Src/hybrid_astar_planner-main/hybrid_astar_planner_plugin.xml
new file mode 100644
index 00000000..d084a05d
--- /dev/null
+++ b/Src/hybrid_astar_planner-main/hybrid_astar_planner_plugin.xml
@@ -0,0 +1,7 @@
+
+
+
+ A implementation of a based planner using Hybrid A*
+
+
+
diff --git a/Src/hybrid_astar_planner-main/include/ReedsShepp.h b/Src/hybrid_astar_planner-main/include/ReedsShepp.h
new file mode 100644
index 00000000..a070c084
--- /dev/null
+++ b/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
+#include
+
+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::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 type(double q0[3], double q1[3]);
+
+ void sample(double q0[3], double q1[3], double step_size, double &length, std::vector > &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
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/include/algorithm.h b/Src/hybrid_astar_planner-main/include/algorithm.h
new file mode 100644
index 00000000..c8d07350
--- /dev/null
+++ b/Src/hybrid_astar_planner-main/include/algorithm.h
@@ -0,0 +1,32 @@
+#ifndef _ALGORITHM_H
+#define _ALGORITHM_H
+
+#include
+#include
+#include
+#include
+#include
+#include "node3d.h"
+#include "node2d.h"
+#include
+#include
+#include
+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
diff --git a/Src/hybrid_astar_planner-main/include/astar.h b/Src/hybrid_astar_planner-main/include/astar.h
new file mode 100644
index 00000000..0be55b43
--- /dev/null
+++ b/Src/hybrid_astar_planner-main/include/astar.h
@@ -0,0 +1,58 @@
+#ifndef _ASTAR_H
+#define _ASTAR_H
+
+#include
+#include "algorithm.h"
+#include "expander.h"
+#include "node2d.h"
+#include
+#include
+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& plan ,ros::Publisher& pub, visualization_msgs::MarkerArray& pathNodes );
+ ~astar(){ }
+ private:
+
+ std::vector 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& plan);
+ /* data */
+};
+
+
+}//end of namespace hybrid_astar_planner
+
+#endif //the end of astar.h
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/include/constants.h b/Src/hybrid_astar_planner-main/include/constants.h
new file mode 100644
index 00000000..6aeb525d
--- /dev/null
+++ b/Src/hybrid_astar_planner-main/include/constants.h
@@ -0,0 +1,84 @@
+#ifndef _CONSTANTS
+#define _CONSTANTS
+
+#include
+
+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
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/include/dubins.h b/Src/hybrid_astar_planner-main/include/dubins.h
new file mode 100644
index 00000000..dd5eafa6
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/include/expander.h b/Src/hybrid_astar_planner-main/include/expander.h
new file mode 100644
index 00000000..00a2e672
--- /dev/null
+++ b/Src/hybrid_astar_planner-main/include/expander.h
@@ -0,0 +1,35 @@
+#ifndef _EXPANDER_H
+#define _EXPANDER_H
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+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& 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
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/include/hybrid_astar.h b/Src/hybrid_astar_planner-main/include/hybrid_astar.h
new file mode 100644
index 00000000..1108fb43
--- /dev/null
+++ b/Src/hybrid_astar_planner-main/include/hybrid_astar.h
@@ -0,0 +1,89 @@
+#ifndef _HYBRID_ASTAR_H
+#define _HYBRID_ASTAR_H
+
+#include
+#include "algorithm.h"
+#include "expander.h"
+#include "node3d.h"
+#include
+#include
+// #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& 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 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& plan);
+ std::unique_ptr grid_a_star_heuristic_generator_;
+};
+
+
+
+
+}//end of namespace hybrid_astar_planner
+
+#endif //the end of astar.h
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/include/node2d.h b/Src/hybrid_astar_planner-main/include/node2d.h
new file mode 100644
index 00000000..9ad298b2
--- /dev/null
+++ b/Src/hybrid_astar_planner-main/include/node2d.h
@@ -0,0 +1,99 @@
+#ifndef _NODE2D_H
+#define _NODE2D_H
+
+#include
+#include
+#include
+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 _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 perd_node = nullptr;
+};
+
+class GridSearch {
+ public:
+ GridSearch()
+ {
+
+ }
+ ~GridSearch()
+ {
+
+ }
+ std::vector> getAdjacentPoints(int cells_x,
+ int cells_y, const unsigned char* charMap, std::shared_ptr point);
+
+ std::unordered_map> GenerateDpMap(
+ const double goal_x, const double goal_y,
+ costmap_2d::Costmap2D* costmap);
+ // std::unordered_map> dp_map_;
+ private:
+
+ // std::unordered_map> dp_map_;
+ struct cmp {
+ // Sorting 3D nodes by increasing C value - the total estimated cost
+ bool operator()(const std::pair& left,
+ const std::pair& right) const {
+ return left.second >= right.second;
+ }
+ };
+};
+
+}//end of namespace hybrid_astar_planner
+
+#endif //end of node2d.h
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/include/node3d.h b/Src/hybrid_astar_planner-main/include/node3d.h
new file mode 100644
index 00000000..a54b9c9c
--- /dev/null
+++ b/Src/hybrid_astar_planner-main/include/node3d.h
@@ -0,0 +1,87 @@
+#ifndef _NODE3D_H
+#define _NODE3D_H
+#include "constants.h"
+#include
+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
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/include/planner_core.h b/Src/hybrid_astar_planner-main/include/planner_core.h
new file mode 100644
index 00000000..c35beb5d
--- /dev/null
+++ b/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
+#include
+#include
+#include
+#include
+#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& plan);
+
+ /**
+ * @brief Publish the plan to RVIZ
+ * @param path the vector contain the path
+ */
+ void publishPlan(const std::vector& path);
+
+ /**
+ * @brief Publish the path node to RVIZ
+ * @param path the vector contain the path
+ */
+ void publishPathNodes(const std::vector& 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
diff --git a/Src/hybrid_astar_planner-main/include/visualize.h b/Src/hybrid_astar_planner-main/include/visualize.h
new file mode 100644
index 00000000..56b8daf9
--- /dev/null
+++ b/Src/hybrid_astar_planner-main/include/visualize.h
@@ -0,0 +1,10 @@
+#ifndef _VISUALIZE_H
+#define _VISUALIZE_H
+#include "node3d.h"
+#include
+#include
+namespace hybrid_astar_planner {
+ void publishSearchNodes(Node3D node,ros::Publisher& pub, visualization_msgs::MarkerArray& pathNodes, int i);
+}
+
+#endif//end of visualize,h
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/package.xml b/Src/hybrid_astar_planner-main/package.xml
new file mode 100644
index 00000000..d317c120
--- /dev/null
+++ b/Src/hybrid_astar_planner-main/package.xml
@@ -0,0 +1,49 @@
+
+
+ hybrid_astar_planner
+ 1.1.2
+ A hybrid_astar_planner package for move_base
+ dengpw
+
+
+
+
+
+ BSD
+
+
+
+
+
+
+ catkin
+ pluginlib
+ pluginlib
+ pluginlib
+
+
+ nav_core
+ roscpp
+ rospy
+ std_msgs
+ visualization_msgs
+ tf
+ ompl
+ map_server
+
+ nav_core
+ roscpp
+ rospy
+ std_msgs
+ visualization_msgs
+ tf
+ ompl
+ map_server
+
+
+
+
+
+
+
+
diff --git a/Src/hybrid_astar_planner-main/src/ReedsShepp.cpp b/Src/hybrid_astar_planner-main/src/ReedsShepp.cpp
new file mode 100644
index 00000000..6234d541
--- /dev/null
+++ b/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
+
+
+namespace
+{
+ // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.
+
+ const double pi = boost::math::constants::pi();
+ const double twopi = 2. * pi;
+ const double RS_EPS = 1e-6;
+ const double ZERO = 10*std::numeric_limits::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::type(double q0[3], double q1[3])
+{
+ ReedsSheppPath path = reedsShepp(q0, q1);
+ std::vector 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 > &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 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];
+}
diff --git a/Src/hybrid_astar_planner-main/src/a_start.cpp b/Src/hybrid_astar_planner-main/src/a_start.cpp
new file mode 100644
index 00000000..b1c585dd
--- /dev/null
+++ b/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
+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& 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> 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 adjacentNodes = gatAdjacentPoints(cells_x, cells_y, charMap, pathNode2D, tmpStart );
+ tmpStart->setClosedSet();
+
+ //下面正式开始A*算法的核心搜索部分
+ for (std::vector::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 astar::gatAdjacentPoints(int cells_x, int cells_y, const unsigned char* charMap, Node2D* pathNode2D, Node2D *point ) {
+ std::vector 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& plan) {
+ Node2D* tmpPtr = node;
+ geometry_msgs::PoseStamped tmpPose;
+ tmpPose.header.stamp = ros::Time::now();
+ //参数后期处理，发布到RViz上进行可视化
+ tmpPose.pose.orientation = tf::createQuaternionMsgFromYaw(0);
+ std::vector 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
diff --git a/Src/hybrid_astar_planner-main/src/algorithm.cpp b/Src/hybrid_astar_planner-main/src/algorithm.cpp
new file mode 100644
index 00000000..3484df68
--- /dev/null
+++ b/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
+#include
+// #include
+// #include
+namespace hybrid_astar_planner {
+
+std::vector gatAdjacentPoints(int cells_x, int cells_y, const unsigned char* charMap, Node2D* pathNode2D, Node2D *point ) {
+ std::vector 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& 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 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 > 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
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/src/dubins.cpp b/Src/hybrid_astar_planner-main/src/dubins.cpp
new file mode 100644
index 00000000..c473ab92
--- /dev/null
+++ b/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
+#include
+
+#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
diff --git a/Src/hybrid_astar_planner-main/src/hybrid_astar.cpp b/Src/hybrid_astar_planner-main/src/hybrid_astar.cpp
new file mode 100644
index 00000000..9ab1847a
--- /dev/null
+++ b/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
+#include "constants.h"
+#include "hybrid_astar.h"
+#include
+#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& plan,
+ ros::Publisher& pub, visualization_msgs::MarkerArray& pathNodes) {
+ #ifdef debug_mode
+ ros::Time t0 = ros::Time::now();
+ #endif
+ // 初始化优先队列，未来改善混合A*算法性能，这里使用的是二项堆优先队列
+ boost::heap::binomial_heap> 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> 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" < open_set;
+ std::unordered_map 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 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::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 hybridAstar::gatAdjacentPoints(int dir, int cells_x,
+ int cells_y, const unsigned char* charMap, Node3D *point) {
+ std::vector 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& plan) {
+ Node3D* tmpPtr = node;
+ geometry_msgs::PoseStamped tmpPose;
+ std::vector 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
diff --git a/Src/hybrid_astar_planner-main/src/node2d.cpp b/Src/hybrid_astar_planner-main/src/node2d.cpp
new file mode 100644
index 00000000..ddb76d69
--- /dev/null
+++ b/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
+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> 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::vector>, cmp>
+ open_pq;
+ std::unordered_map> open_set;
+ std::unordered_map> dp_map_;
+ std::shared_ptr goal_node = std::make_shared(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 current_node = open_set[id];
+ dp_map_.emplace(current_node->getindex(cells_x), current_node);
+ std::vector> adjacent_nodes =
+ getAdjacentPoints(cells_x, cells_y, charMap, current_node );
+ for (std::vector>::iterator
+ it = adjacent_nodes.begin(); it != adjacent_nodes.end(); ++it) {
+ std::shared_ptr 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> GridSearch::getAdjacentPoints(int cells_x,
+ int cells_y, const unsigned char* charMap, std::shared_ptr point) {
+ std::vector> 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 node = std::make_shared(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
diff --git a/Src/hybrid_astar_planner-main/src/node3d.cpp b/Src/hybrid_astar_planner-main/src/node3d.cpp
new file mode 100644
index 00000000..9b570099
--- /dev/null
+++ b/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
+#include
+// 设计启发函数
+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
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/src/planner_core.cpp b/Src/hybrid_astar_planner-main/src/planner_core.cpp
new file mode 100644
index 00000000..59923320
--- /dev/null
+++ b/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
+#include "planner_core.h"
+#include
+#include
+#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("plan", 1);
+ path_vehicles_pub_ = private_nh.advertise("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& 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
+
diff --git a/Src/hybrid_astar_planner-main/src/visualize.cpp b/Src/hybrid_astar_planner-main/src/visualize.cpp
new file mode 100644
index 00000000..2f45ec2d
--- /dev/null
+++ b/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
+namespace hybrid_astar_planner {
+
+void HybridAStarPlanner::publishPlan(const std::vector& 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& 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
+#include
+#include
+#include
+#include
+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 planner_;
+ //路径规划的vector类模板，用来保存路径
+ std::vector* 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
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/launch/config.rviz b/Src/hybrid_astar_planner-main/test_the_plugin/launch/config.rviz
new file mode 100644
index 00000000..a9c9b9be
--- /dev/null
+++ b/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:
+ 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:
+ 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
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/launch/test.launch b/Src/hybrid_astar_planner-main/test_the_plugin/launch/test.launch
new file mode 100644
index 00000000..abafd3e1
--- /dev/null
+++ b/Src/hybrid_astar_planner-main/test_the_plugin/launch/test.launch
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/maps/map.yaml b/Src/hybrid_astar_planner-main/test_the_plugin/maps/map.yaml
new file mode 100644
index 00000000..957d4750
--- /dev/null
+++ b/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
+
+
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/maps/map_demo.png b/Src/hybrid_astar_planner-main/test_the_plugin/maps/map_demo.png
new file mode 100644
index 00000000..778ebfa9
Binary files /dev/null and b/Src/hybrid_astar_planner-main/test_the_plugin/maps/map_demo.png differ
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/maps/mm.pgm b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mm.pgm
new file mode 100644
index 00000000..2c830d2e
--- /dev/null
+++ b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mm.pgm
@@ -0,0 +1,5 @@
+P5
+# CREATOR: map_saver.cpp 0.050 m/pix
+448 672
+255
+
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/maps/mm.yaml b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mm.yaml
new file mode 100644
index 00000000..d4832149
--- /dev/null
+++ b/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
+
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap.pgm b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap.pgm
new file mode 100644
index 00000000..ff5bb085
--- /dev/null
+++ b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap.pgm
@@ -0,0 +1,5 @@
+P5
+# CREATOR: map_saver.cpp 0.050 m/pix
+1984 1984
+255
+
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap.yaml b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap.yaml
new file mode 100644
index 00000000..3018cc80
--- /dev/null
+++ b/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
+
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap2077.pgm b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap2077.pgm
new file mode 100644
index 00000000..e9f90196
--- /dev/null
+++ b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap2077.pgm
@@ -0,0 +1,5 @@
+P5
+# CREATOR: map_saver.cpp 0.050 m/pix
+288 512
+255
+
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap2077.yaml b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap2077.yaml
new file mode 100644
index 00000000..6a395155
--- /dev/null
+++ b/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
+
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap2088.pgm b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap2088.pgm
new file mode 100644
index 00000000..3e282e7e
--- /dev/null
+++ b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap2088.pgm
@@ -0,0 +1,5 @@
+P5
+# CREATOR: map_saver.cpp 0.050 m/pix
+288 288
+255
+
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap2088.yaml b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap2088.yaml
new file mode 100644
index 00000000..f9bbdb38
--- /dev/null
+++ b/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
+
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap4.pgm b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap4.pgm
new file mode 100644
index 00000000..abbf26fa
--- /dev/null
+++ b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap4.pgm
@@ -0,0 +1,5 @@
+P5
+# CREATOR: map_saver.cpp 0.050 m/pix
+1984 1984
+255
+
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap4.yaml b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap4.yaml
new file mode 100644
index 00000000..6ec1bd70
--- /dev/null
+++ b/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
+
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap5.pgm b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap5.pgm
new file mode 100644
index 00000000..fb8a7e4b
--- /dev/null
+++ b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap5.pgm
@@ -0,0 +1,5 @@
+P5
+# CREATOR: map_saver.cpp 0.050 m/pix
+672 448
+255
+
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap5.yaml b/Src/hybrid_astar_planner-main/test_the_plugin/maps/mymap5.yaml
new file mode 100644
index 00000000..09426b3e
--- /dev/null
+++ b/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
+
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/param/base_local_planner_params.yaml b/Src/hybrid_astar_planner-main/test_the_plugin/param/base_local_planner_params.yaml
new file mode 100644
index 00000000..ee11a5fe
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/param/costmap_common_params.yaml b/Src/hybrid_astar_planner-main/test_the_plugin/param/costmap_common_params.yaml
new file mode 100644
index 00000000..8950ac10
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/param/costmap_common_params_mark.txt b/Src/hybrid_astar_planner-main/test_the_plugin/param/costmap_common_params_mark.txt
new file mode 100644
index 00000000..9376ce7f
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/param/global_costmap_params.yaml b/Src/hybrid_astar_planner-main/test_the_plugin/param/global_costmap_params.yaml
new file mode 100644
index 00000000..c653287d
--- /dev/null
+++ b/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"}
+
+
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/param/local_costmap_params.yaml b/Src/hybrid_astar_planner-main/test_the_plugin/param/local_costmap_params.yaml
new file mode 100644
index 00000000..7be342e1
--- /dev/null
+++ b/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
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/param/move_base_params.yaml b/Src/hybrid_astar_planner-main/test_the_plugin/param/move_base_params.yaml
new file mode 100644
index 00000000..c94122fe
--- /dev/null
+++ b/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
+
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/param/plugins.yaml b/Src/hybrid_astar_planner-main/test_the_plugin/param/plugins.yaml
new file mode 100644
index 00000000..3501cae3
--- /dev/null
+++ b/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"}
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/src/test.cpp b/Src/hybrid_astar_planner-main/test_the_plugin/src/test.cpp
new file mode 100644
index 00000000..47a7fbe4
--- /dev/null
+++ b/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
+#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;
+}
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/src/test_plugins.cpp b/Src/hybrid_astar_planner-main/test_the_plugin/src/test_plugins.cpp
new file mode 100644
index 00000000..17484c09
--- /dev/null
+++ b/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
+#include
+#include
+#include
+#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 bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"); //导入插件
+ planner_plan_ = new std::vector();
+ //这个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" <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;
+ }
+}
\ No newline at end of file
diff --git a/Src/hybrid_astar_planner-main/test_the_plugin/src/tf_broadcaster.cpp b/Src/hybrid_astar_planner-main/test_the_plugin/src/tf_broadcaster.cpp
new file mode 100644
index 00000000..5a1e4932
--- /dev/null
+++ b/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
+#include
+#include
+#include
+#include
+// 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();
+ }
+}