Create ROS global planner plugin from Hybrid A Star

.gitignore

@@ -1,44 +1,44 @@
-# Created by .ignore support plugin (hsz.mobi)
-### C++ template
-# Prerequisites
-*.d
-
-# Compiled Object files
-*.slo
-*.lo
-*.o
-*.obj
-
-# Precompiled Headers
-*.gch
-*.pch
-
-# Compiled Dynamic libraries
-*.so
-*.dylib
-*.dll
-
-# Fortran module files
-*.mod
-*.smod
-
-# Compiled Static libraries
-*.lai
-*.la
-*.a
-*.lib
-
-# Executables
-*.exe
-*.out
-*.app
-
-**/cmake-build-debug
-**/build
-**/.idea
-/bin
-*/data
-*/dataset
-.catkin_workspace
-devel/
-build/
+# Created by .ignore support plugin (hsz.mobi)
+### C++ template
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+**/cmake-build-debug
+**/build
+**/.idea
+/bin
+*/data
+*/dataset
+.catkin_workspace
+devel/
+build/

CMakeLists.txt

@@ -37,6 +37,7 @@ find_package(
     nav_msgs
     roscpp
     tf
+    nav_core
 )
 
 include_directories(
@@ -45,27 +46,53 @@ include_directories(
 )
 
 catkin_package(
-    #  INCLUDE_DIRS include
-    #  LIBRARIES path_searcher
+      INCLUDE_DIRS include
+      LIBRARIES hybrid_a_star 
     #  CATKIN_DEPENDS nav_mags roscpp tf
     #  DEPENDS system_lib
 )
 
 add_library(
-    PATH_SEARCHER_LIB SHARED
+    ${PROJECT_NAME}
     src/rs_path.cpp
     src/costmap_subscriber.cpp
     src/goal_pose_subscriber.cpp
     src/init_pose_subscriber.cpp
     src/hybrid_a_star.cpp
     src/hybrid_a_star_flow.cpp
+<<<<<<< HEAD
+    #src/hybrid_astar_global_planner.cpp
+=======
+>>>>>>> f3c30d717d4183ec46414153084b232c2d006dc3
+)
+
+add_library(hybrid_astar_global_planner
+   src/hybrid_astar_global_planner.cpp)
+   target_link_libraries(hybrid_astar_global_planner
+   ${catkin_LIBRARIES} hybrid_a_star
 )
 
 target_link_libraries(
-    PATH_SEARCHER_LIB
+    ${PROJECT_NAME}
     ${catkin_LIBRARIES}
     ${GLOG_LIBRARIES}
 )
 
 add_executable(run_hybrid_astar app/run_hybrid_astar.cpp)
-target_link_libraries(run_hybrid_astar PATH_SEARCHER_LIB)
+target_link_libraries(run_hybrid_astar ${PROJECT_NAME})
+
+install(
+  TARGETS ${PROJECT_NAME} 
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+install(
+  DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+install(
+  FILES bgp_plugin.xml 
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

LICENSE

@@ -1,30 +1,30 @@
-The Hybrid A star is released as Open Source
-under the terms of a 3-clause BSD license.
-
-Copyright 2022 Zhang Zhimeng
-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,
+The Hybrid A star is released as Open Source
+under the terms of a 3-clause BSD license.
+
+Copyright 2022 Zhang Zhimeng
+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.

app/run_hybrid_astar.cpp

@@ -1,55 +1,55 @@
-/*******************************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2022 Zhang Zhimeng
- * 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.
- *
- * 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.
- ******************************************************************************/
-
-#include "hybrid_a_star/hybrid_a_star_flow.h"
-
-#include "3rd/backward.hpp"
-
-#include <ros/ros.h>
-
-namespace backward {
-backward::SignalHandling sh;
-}
-
-int main(int argc, char **argv) {
-    ros::init(argc, argv, "run_hybrid_astar");
-    ros::NodeHandle node_handle("~");
-
-    HybridAStarFlow kinodynamic_astar_flow(node_handle);
-
-    ros::Rate rate(10);
-
-    while (ros::ok()) {
-        kinodynamic_astar_flow.Run();
-
-        ros::spinOnce();
-        rate.sleep();
-    }
-
-    ros::shutdown();
-    return 0;
+/*******************************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2022 Zhang Zhimeng
+ * 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.
+ *
+ * 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.
+ ******************************************************************************/
+
+#include "hybrid_a_star/hybrid_a_star_flow.h"
+
+#include "3rd/backward.hpp"
+
+#include <ros/ros.h>
+
+namespace backward {
+backward::SignalHandling sh;
+}
+
+int main(int argc, char **argv) {
+    ros::init(argc, argv, "run_hybrid_astar");
+    ros::NodeHandle node_handle("~");
+
+    HybridAStarFlow kinodynamic_astar_flow(node_handle);
+
+    ros::Rate rate(10);
+
+    while (ros::ok()) {
+        kinodynamic_astar_flow.Run();
+
+        ros::spinOnce();
+        rate.sleep();
+    }
+
+    ros::shutdown();
+    return 0;
 }

bgp_plugin.xml

@@ -0,0 +1,9 @@
+<library path="lib/libhybrid_astar_global_planner">
+  <class
+    name="hybrid_astar_global_planner/HybridAstarGlobalPlanner"
+    type="hybrid_astar_global_planner::HybridAstarGlobalPlanner"
+    base_class_type="nav_core::BaseGlobalPlanner">
+
+    <description>This is a global planner plugin for navgraph-based path generation.</description>
+  </class>
+</library>