├── map └── maze_5cm │ ├── map.pgm │ └── map.yaml ├── params ├── lattice_path_planner_params.yaml ├── global_costmap_params.yaml └── costmap_common_params.yaml ├── README.md ├── bgp_plugin.xml ├── .editorconfig ├── .pre-commit-config.yaml ├── .clang-format ├── launch ├── demo.launch └── demo.rviz ├── LICENSE ├── CMakeLists.txt ├── lattice_path_planner ├── common │ ├── constants.h │ ├── node.h │ ├── footprint_helper.h │ ├── heap.h │ ├── breadth_first_search.h │ ├── heap.cc │ ├── breadth_first_search.cc │ ├── utils.cc │ ├── footprint_helper.cc │ └── utils.h ├── grid_search │ ├── node2d.h │ ├── grid_search.h │ └── grid_search.cc ├── primitive_generator │ ├── primitive_generator.h │ └── primitive_generator.cc ├── lattice_a_star │ ├── node3d.h │ ├── lattice_a_star.h │ └── lattice_a_star.cc ├── path_visualizer.h ├── lattice_path_planner_ros.h ├── tf_broadcaster.cc ├── lattice_path_planner_ros_test.cc ├── path_visualizer.cc └── lattice_path_planner_ros.cc ├── .clang-tidy ├── package.xml └── third_party └── spline └── spline.h /map/maze_5cm/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nkuwenjian/lattice_path_planner/HEAD/map/maze_5cm/map.pgm -------------------------------------------------------------------------------- /map/maze_5cm/map.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.050000 3 | origin: [-19.000000, -19.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /params/lattice_path_planner_params.yaml: -------------------------------------------------------------------------------- 1 | LatticePathPlannerROS: 2 | nominalvel_mpersecs: 0.4 3 | timetoturn45degsinplace_secs: 0.6 4 | min_sample_interval: 0.1 5 | treat_unknown_as_free: true 6 | 7 | vehicle_length: 0.23 8 | vehicle_width: 0.2 9 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # State lattice-based path planner 2 | This repo contains a global planner plugin of state lattice-based path planner for ROS navigation stack. Specifically, global path planning problems are addressed by A* search algorithm combined with motion primitives. The original code refers to [SBPL](https://github.com/sbpl/sbpl). 3 | -------------------------------------------------------------------------------- /params/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: base_link 4 | update_frequency: 1.0 5 | publish_frequency: 0.5 6 | 7 | transform_tolerance: 0.5 8 | plugins: 9 | - { name: static_layer, type: "costmap_2d::StaticLayer" } 10 | - { name: inflation_layer, type: "costmap_2d::InflationLayer" } 11 | -------------------------------------------------------------------------------- /bgp_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | An implementation of search-based path planning with motion primitives. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /.editorconfig: -------------------------------------------------------------------------------- 1 | # http://editorconfig.org 2 | root = true 3 | 4 | [*] 5 | charset = utf-8 6 | # like -i=2 7 | indent_style = space 8 | indent_size = 2 9 | insert_final_newline = true 10 | trim_trailing_whitespace = true 11 | 12 | [*.{sh,bash,bashrc}] 13 | shell_variant = bash # like -ln=bash 14 | binary_next_line = true # like -bn 15 | switch_case_indent = true # like -ci 16 | space_redirects = true # like -sr 17 | 18 | # in case third party libraries need a Makefile. 19 | [Makefile] 20 | indent_style = tab 21 | -------------------------------------------------------------------------------- /params/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | #---standard pioneer footprint--- 3 | #---(in meters)--- 4 | # robot_radius: 0.25 5 | footprint: [[-0.23, -0.2], [-0.23, 0.2], [0.23, 0.2], [0.23, -0.2]] 6 | footprint_padding: 0.01 7 | 8 | transform_tolerance: 0.2 9 | 10 | always_send_full_costmap: true 11 | 12 | static_layer: 13 | enabled: true 14 | map_topic: "/map" 15 | 16 | inflation_layer: 17 | enabled: true 18 | cost_scaling_factor: 10.0 # exponential rate at which the obstacle cost drops off (default: 10) 19 | inflation_radius: 0.5 # max. distance from an obstacle at which costs are incurred for planning paths (default: 0.55) 20 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # See https://pre-commit.com for more information 2 | # See https://pre-commit.com/hooks.html for more hooks 3 | exclude: 'third_party/.*|map/.*' 4 | repos: 5 | - repo: https://github.com/pre-commit/pre-commit-hooks 6 | rev: v3.2.0 7 | hooks: 8 | - id: trailing-whitespace 9 | - id: end-of-file-fixer 10 | - id: check-added-large-files 11 | - id: check-merge-conflict 12 | - repo: https://github.com/pre-commit/mirrors-clang-format 13 | rev: "v9.0.0" 14 | hooks: 15 | - id: clang-format 16 | types_or: [c++] 17 | - repo: https://github.com/cpplint/cpplint 18 | rev: 1.6.1 19 | hooks: 20 | - id: cpplint 21 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | --- 4 | Language: Cpp 5 | DerivePointerAlignment: false 6 | PointerAlignment: Left 7 | Cpp11BracedListStyle: true 8 | Standard: Cpp11 9 | CommentPragmas: '^ NOLINT' 10 | # Mimic cpplint style 11 | IncludeCategories: 12 | # Note that the "main" header is priority 0 13 | # The priority is assigned to first match in the ordered list 14 | # Miscelaneous system libraries 15 | - Regex: '<(cxxabi.h|immintrin.h|malloc.h|wait.h|x86intrin.h|cuda.*)>' 16 | Priority: 3 17 | # third-party libraries 18 | - Regex: '<(tinyxml2.h)>' 19 | Priority: 3 20 | # C standard libraries 21 | - Regex: '<(arpa/|netinet/|net/if|sys/)?[^\./]*\.h>' 22 | Priority: 1 23 | # C++ standard libraries 24 | - Regex: '<[^/\./]*>' 25 | Priority: 2 26 | # Experimental or other system libraries 27 | - Regex: '<' 28 | Priority: 3 29 | # Test libs 30 | - Regex: '"(gtest|gmock)/' 31 | Priority: 4 32 | # Protobuf Files 33 | - Regex: '\.pb\.h' 34 | Priority: 6 35 | # Lattice path planner libs 36 | - Regex: '^"(lattice_path_planner)' 37 | Priority: 7 38 | # The rest 39 | - Regex: '.*' 40 | Priority: 5 41 | --- 42 | -------------------------------------------------------------------------------- /launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2023, NKU Mobile & Flying Robotics Lab 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(lattice_path_planner) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | message(STATUS "BUILD TYPE: " ${CMAKE_BUILD_TYPE}) 6 | 7 | add_compile_options(-std=c++14) 8 | 9 | # Compile ROS package 10 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror -Wall") 11 | 12 | # set(CMAKE_CXX_CLANG_TIDY 13 | # clang-tidy; 14 | # -header-filter=${PROJECT_SOURCE_DIR}/lattice_path_planner; 15 | # -warnings-as-errors=*; 16 | # ) 17 | 18 | find_package(catkin REQUIRED COMPONENTS 19 | costmap_2d 20 | geometry_msgs 21 | nav_core 22 | nav_msgs 23 | pluginlib 24 | tf2_geometry_msgs 25 | tf2_ros 26 | ) 27 | 28 | # Glog 29 | find_package(glog REQUIRED) 30 | 31 | catkin_package( 32 | INCLUDE_DIRS ${PROJECT_SOURCE_DIR} 33 | LIBRARIES lattice_path_planner 34 | CATKIN_DEPENDS 35 | costmap_2d 36 | geometry_msgs 37 | nav_core 38 | nav_msgs 39 | pluginlib 40 | tf2_geometry_msgs 41 | tf2_ros 42 | ) 43 | 44 | include_directories( 45 | ${PROJECT_SOURCE_DIR} 46 | third_party 47 | ${catkin_INCLUDE_DIRS} 48 | ) 49 | 50 | add_library(lattice_path_planner 51 | lattice_path_planner/common/breadth_first_search.cc 52 | lattice_path_planner/common/footprint_helper.cc 53 | lattice_path_planner/common/heap.cc 54 | lattice_path_planner/common/utils.cc 55 | lattice_path_planner/grid_search/grid_search.cc 56 | lattice_path_planner/lattice_a_star/lattice_a_star.cc 57 | lattice_path_planner/primitive_generator/primitive_generator.cc 58 | lattice_path_planner/lattice_path_planner_ros.cc 59 | lattice_path_planner/path_visualizer.cc 60 | ) 61 | target_link_libraries(lattice_path_planner ${catkin_LIBRARIES} glog::glog) 62 | 63 | add_executable(lattice_path_planner_test lattice_path_planner/lattice_path_planner_ros_test.cc) 64 | target_link_libraries(lattice_path_planner_test lattice_path_planner ${catkin_LIBRARIES}) 65 | 66 | add_executable(lattice_path_planner_tf_broadcaster lattice_path_planner/tf_broadcaster.cc) 67 | target_link_libraries(lattice_path_planner_tf_broadcaster ${catkin_LIBRARIES} glog::glog) 68 | -------------------------------------------------------------------------------- /lattice_path_planner/common/constants.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2023, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | * Author: Jian Wen (nkuwenjian@gmail.com) 32 | *****************************************************************************/ 33 | 34 | #pragma once 35 | 36 | #include 37 | 38 | namespace lattice_path_planner { 39 | namespace common { 40 | 41 | static constexpr int kInfiniteCost = std::numeric_limits::max(); 42 | static constexpr int kPhiGridResolution = 16; 43 | static constexpr int kNumOfGridSearchActions = 16; 44 | static constexpr std::size_t kInitHeapCapacity = 5000; 45 | 46 | } // namespace common 47 | } // namespace lattice_path_planner 48 | -------------------------------------------------------------------------------- /lattice_path_planner/common/node.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #pragma once 33 | 34 | namespace lattice_path_planner { 35 | namespace common { 36 | 37 | class Node { 38 | public: 39 | enum class NodeStatus { kOpen, kClosed }; 40 | 41 | Node() = default; 42 | virtual ~Node() = default; 43 | 44 | std::size_t heap_index() const { return heap_index_; } 45 | void set_heap_index(const std::size_t heap_index) { 46 | heap_index_ = heap_index; 47 | } 48 | 49 | protected: 50 | std::size_t heap_index_ = 0; 51 | }; 52 | 53 | } // namespace common 54 | } // namespace lattice_path_planner 55 | -------------------------------------------------------------------------------- /lattice_path_planner/common/footprint_helper.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #pragma once 33 | 34 | #include 35 | #include 36 | 37 | #include "lattice_path_planner/common/utils.h" 38 | 39 | namespace lattice_path_planner { 40 | namespace common { 41 | 42 | class FootprintHelper { 43 | public: 44 | FootprintHelper() = delete; 45 | 46 | static XYCellBounds GetMotionXYCells(const std::vector& polygon, 47 | const std::vector& poses, 48 | std::vector* cells, double res); 49 | 50 | static void GetFootprintXYCells(const std::vector& polygon, 51 | std::vector* cells, 52 | const XYThetaPoint& pose, double res); 53 | 54 | static XYCellBounds GetFootprintXYCells(const std::vector& polygon, 55 | std::set* cells, 56 | const XYThetaPoint& pose, double res); 57 | }; 58 | 59 | } // namespace common 60 | } // namespace lattice_path_planner 61 | -------------------------------------------------------------------------------- /lattice_path_planner/common/heap.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #pragma once 33 | 34 | #include 35 | 36 | #include "lattice_path_planner/common/constants.h" 37 | #include "lattice_path_planner/common/node.h" 38 | #include "lattice_path_planner/common/utils.h" 39 | 40 | namespace lattice_path_planner { 41 | namespace common { 42 | 43 | struct HeapElement { 44 | Node* node = nullptr; 45 | int key = 0; 46 | }; 47 | 48 | class Heap { 49 | public: 50 | Heap(); 51 | explicit Heap(std::size_t capacity); 52 | virtual ~Heap(); 53 | 54 | bool Empty() const { return size_ == 0; } 55 | std::size_t Size() const { return size_; } 56 | void Clear(); 57 | void Insert(Node* node, int key); 58 | void Update(Node* node, int new_key); 59 | int GetMinKey() const { return Empty() ? kInfiniteCost : queue_[1].key; } 60 | Node* Pop(); 61 | 62 | private: 63 | void PercolateUp(std::size_t hole, HeapElement obj); 64 | void PercolateDown(std::size_t hole, HeapElement obj); 65 | void PercolateUpOrDown(std::size_t hole, HeapElement obj); 66 | void Allocate(); 67 | 68 | std::size_t size_ = 0; 69 | std::size_t capacity_ = kInitHeapCapacity; 70 | std::vector queue_; 71 | }; 72 | 73 | } // namespace common 74 | } // namespace lattice_path_planner 75 | -------------------------------------------------------------------------------- /lattice_path_planner/common/breadth_first_search.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #pragma once 33 | 34 | #include 35 | #include 36 | 37 | #include "glog/logging.h" 38 | 39 | #include "lattice_path_planner/common/utils.h" 40 | 41 | namespace lattice_path_planner { 42 | namespace common { 43 | 44 | class BreadthFirstSearch { 45 | public: 46 | BreadthFirstSearch(int size_x, int size_y, int obs_thresh); 47 | virtual ~BreadthFirstSearch() = default; 48 | 49 | bool ComputeDistanceFromPoint(const std::vector>& grid, 50 | int x, int y); 51 | 52 | bool ComputeDistanceFromPoints(const std::vector>& grid, 53 | const std::vector& x, 54 | const std::vector& y); 55 | 56 | void ComputeDistanceFromObstacles(const std::vector>& grid); 57 | 58 | int GetDistance(int x, int y) const { return dist_[x][y]; } 59 | 60 | private: 61 | void ClearDistances(); 62 | 63 | void ComputeDistances(const std::vector>& grid); 64 | 65 | std::vector> dist_; 66 | std::queue queue_; 67 | int size_x_; 68 | int size_y_; 69 | int obs_thresh_; 70 | 71 | const std::size_t num_of_actions_ = 8U; 72 | std::vector dx_; 73 | std::vector dy_; 74 | }; 75 | 76 | } // namespace common 77 | } // namespace lattice_path_planner 78 | -------------------------------------------------------------------------------- /.clang-tidy: -------------------------------------------------------------------------------- 1 | # Copied from Google glog project https://github.com/google/glog, to be customized later. 2 | 3 | # NOTE: following checks are disabled: 4 | # -modernize-use-trailing-return-type 5 | # -readability-magic-numbers 6 | 7 | Checks: ' 8 | android-*, 9 | bugprone-*, 10 | clang-analyzer-*, 11 | clang-diagnostic-*, 12 | darwin-*, 13 | google-*, 14 | modernize-*, 15 | -modernize-use-trailing-return-type, 16 | performance-*, 17 | portability-*, 18 | readability-*, 19 | -readability-magic-numbers, 20 | ' 21 | WarningsAsErrors: '' 22 | HeaderFilterRegex: '' 23 | AnalyzeTemporaryDtors: false 24 | FormatStyle: file 25 | CheckOptions: 26 | - key: cert-dcl16-c.NewSuffixes 27 | value: 'L;LL;LU;LLU' 28 | - key: cert-oop54-cpp.WarnOnlyIfThisHasSuspiciousField 29 | value: '0' 30 | - key: cppcoreguidelines-explicit-virtual-functions.IgnoreDestructors 31 | value: '1' 32 | - key: cppcoreguidelines-non-private-member-variables-in-classes.IgnoreClassesWithAllMemberVariablesBeingPublic 33 | value: '1' 34 | - key: google-build-namespaces.HeaderFileExtensions 35 | value: ',h,hh,hpp,hxx' 36 | - key: google-global-names-in-headers.HeaderFileExtensions 37 | value: ',h,hh,hpp,hxx' 38 | - key: google-readability-braces-around-statements.ShortStatementLines 39 | value: '1' 40 | - key: google-readability-function-size.BranchThreshold 41 | value: '4294967295' 42 | - key: google-readability-function-size.LineThreshold 43 | value: '4294967295' 44 | - key: google-readability-function-size.NestingThreshold 45 | value: '4294967295' 46 | - key: google-readability-function-size.ParameterThreshold 47 | value: '4294967295' 48 | - key: google-readability-function-size.StatementThreshold 49 | value: '800' 50 | - key: google-readability-function-size.VariableThreshold 51 | value: '4294967295' 52 | - key: google-readability-namespace-comments.ShortNamespaceLines 53 | value: '10' 54 | - key: google-readability-namespace-comments.SpacesBeforeComments 55 | value: '2' 56 | - key: google-runtime-int.SignedTypePrefix 57 | value: int 58 | - key: google-runtime-int.TypeSuffix 59 | value: '' 60 | - key: google-runtime-int.UnsignedTypePrefix 61 | value: uint 62 | - key: google-runtime-references.WhiteListTypes 63 | value: '' 64 | - key: modernize-loop-convert.MaxCopySize 65 | value: '16' 66 | - key: modernize-loop-convert.MinConfidence 67 | value: reasonable 68 | - key: modernize-loop-convert.NamingStyle 69 | value: CamelCase 70 | - key: modernize-pass-by-value.IncludeStyle 71 | value: llvm 72 | - key: modernize-replace-auto-ptr.IncludeStyle 73 | value: llvm 74 | - key: modernize-use-nullptr.NullMacros 75 | value: 'NULL' 76 | ... 77 | -------------------------------------------------------------------------------- /lattice_path_planner/grid_search/node2d.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #pragma once 33 | 34 | #include 35 | 36 | #include "lattice_path_planner/common/constants.h" 37 | #include "lattice_path_planner/common/node.h" 38 | #include "lattice_path_planner/common/utils.h" 39 | 40 | namespace lattice_path_planner { 41 | namespace grid_search { 42 | 43 | class Node2d : public common::Node { 44 | public: 45 | Node2d() = default; 46 | Node2d(const int grid_x, const int grid_y) 47 | : grid_x_(grid_x), grid_y_(grid_y) {} 48 | ~Node2d() override = default; 49 | void set_g(const int g) { g_ = g; } 50 | void set_h(const int h) { h_ = h; } 51 | void set_pre_node(const Node2d* pre_node) { pre_node_ = pre_node; } 52 | void set_iterations(const std::size_t iterations) { 53 | iterations_ = iterations; 54 | } 55 | int grid_x() const { return grid_x_; } 56 | int grid_y() const { return grid_y_; } 57 | int g() const { return g_; } 58 | int h() const { return h_; } 59 | std::size_t iterations() const { return iterations_; } 60 | const Node2d* pre_node() const { return pre_node_; } 61 | bool operator==(const Node2d& rhs) const { 62 | return grid_x_ == rhs.grid_x_ && grid_y_ == rhs.grid_y_; 63 | } 64 | 65 | private: 66 | int grid_x_ = 0; 67 | int grid_y_ = 0; 68 | int g_ = common::kInfiniteCost; 69 | int h_ = 0; 70 | const Node2d* pre_node_ = nullptr; 71 | std::size_t iterations_ = 0U; 72 | }; 73 | 74 | } // namespace grid_search 75 | } // namespace lattice_path_planner 76 | -------------------------------------------------------------------------------- /lattice_path_planner/primitive_generator/primitive_generator.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #pragma once 33 | 34 | #include 35 | #include 36 | 37 | #include "lattice_path_planner/common/utils.h" 38 | 39 | namespace lattice_path_planner { 40 | namespace primitive_generator { 41 | 42 | // data read from mprim file 43 | struct Primitive { 44 | int id; 45 | uint8_t start_grid_phi; 46 | int multiplier; 47 | unsigned int cost; 48 | int end_grid_x; 49 | int end_grid_y; 50 | int end_grid_phi; // end_grid_phi is in the range of [0, 16) 51 | 52 | std::vector intersectingcellsV; 53 | 54 | // intermptV start at 0,0,starttheta and end at endcell in continuous 55 | // domain with half-bin less to account for 0,0 start 56 | std::vector intermptV; 57 | 58 | // start at 0,0,starttheta and end at endcell in discrete domain 59 | std::vector interm2DcellsV; 60 | }; 61 | 62 | class PrimitiveGenerator { 63 | public: 64 | explicit PrimitiveGenerator(const common::EnvironmentConfig& env_cfg); 65 | virtual ~PrimitiveGenerator() = default; 66 | 67 | void Init(const std::string& file); 68 | const std::vector>& motion_primitives() const { 69 | return motion_primitives_; 70 | } 71 | 72 | private: 73 | bool ReadMotionPrimitives(const std::string& file); 74 | void ReadinMotionPrimitive(std::ifstream* fin, Primitive* prim) const; 75 | static void ReadinPose(std::ifstream* fin, double* x, double* y, double* phi); 76 | void PrecomputeActionswithCompleteMotionPrimitive(); 77 | 78 | const common::EnvironmentConfig& env_cfg_; 79 | int phi_grid_resolution_; 80 | int action_width_; 81 | std::vector> motion_primitives_; 82 | common::XYCellBounds polygon_bounds_; 83 | }; 84 | 85 | } // namespace primitive_generator 86 | } // namespace lattice_path_planner 87 | -------------------------------------------------------------------------------- /lattice_path_planner/lattice_a_star/node3d.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #pragma once 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | #include "lattice_path_planner/common/constants.h" 39 | #include "lattice_path_planner/common/node.h" 40 | #include "lattice_path_planner/common/utils.h" 41 | 42 | namespace lattice_path_planner { 43 | namespace lattice_a_star { 44 | 45 | class Node3d : public common::Node { 46 | public: 47 | Node3d(const int grid_x, const int grid_y, const int grid_phi) 48 | : grid_x_(grid_x), grid_y_(grid_y), grid_phi_(grid_phi) {} 49 | ~Node3d() override = default; 50 | void set_g(const int g) { g_ = g; } 51 | void set_h(const int h) { h_ = h; } 52 | void set_pre_node(const Node3d* pre_node) { pre_node_ = pre_node; } 53 | void set_iterations(const std::size_t iterations) { 54 | iterations_ = iterations; 55 | } 56 | int grid_x() const { return grid_x_; } 57 | int grid_y() const { return grid_y_; } 58 | int grid_phi() const { return grid_phi_; } 59 | int g() const { return g_; } 60 | int h() const { return h_; } 61 | std::size_t iterations() const { return iterations_; } 62 | void set_action_idx(std::pair&& action_idx) { 63 | action_idx_ = std::move(action_idx); 64 | } 65 | const std::pair& action_idx() const { return action_idx_; } 66 | const Node3d* pre_node() const { return pre_node_; } 67 | bool operator==(const Node3d& rhs) const { 68 | return grid_x_ == rhs.grid_x_ && grid_y_ == rhs.grid_y_ && 69 | grid_phi_ == rhs.grid_phi_; 70 | } 71 | 72 | private: 73 | int grid_x_ = 0; 74 | int grid_y_ = 0; 75 | int grid_phi_ = 0; 76 | int g_ = common::kInfiniteCost; 77 | int h_ = 0; 78 | const Node3d* pre_node_ = nullptr; 79 | std::pair action_idx_; 80 | std::size_t iterations_ = 0U; 81 | }; 82 | 83 | } // namespace lattice_a_star 84 | } // namespace lattice_path_planner 85 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lattice_path_planner 4 | 0.0.0 5 | The lattice_path_planner package 6 | 7 | 8 | 9 | 10 | Jian Wen 11 | 12 | 13 | 14 | 15 | 16 | BSD-3-Clause license 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | costmap_2d 53 | geometry_msgs 54 | nav_core 55 | nav_msgs 56 | pluginlib 57 | tf2_geometry_msgs 58 | tf2_ros 59 | costmap_2d 60 | geometry_msgs 61 | nav_core 62 | nav_msgs 63 | pluginlib 64 | tf2_geometry_msgs 65 | tf2_ros 66 | costmap_2d 67 | geometry_msgs 68 | nav_core 69 | nav_msgs 70 | pluginlib 71 | tf2_geometry_msgs 72 | tf2_ros 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /lattice_path_planner/path_visualizer.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #pragma once 33 | 34 | #include 35 | #include 36 | 37 | #include "geometry_msgs/Pose.h" 38 | #include "nav_msgs/Path.h" 39 | #include "ros/ros.h" 40 | #include "tf/tf.h" 41 | #include "visualization_msgs/MarkerArray.h" 42 | 43 | namespace lattice_path_planner { 44 | 45 | struct Color { 46 | float red; 47 | float green; 48 | float blue; 49 | }; 50 | 51 | static constexpr Color kTeal = {102.F / 255.F, 217.F / 255.F, 239.F / 255.F}; 52 | static constexpr Color kGreen = {166.F / 255.F, 226.F / 255.F, 46.F / 255.F}; 53 | static constexpr Color kOrange = {253.F / 255.F, 151.F / 255.F, 31.F / 255.F}; 54 | static constexpr Color kPink = {249.F / 255.F, 38.F / 255.F, 114.F / 255.F}; 55 | static constexpr Color kPurple = {174.F / 255.F, 129.F / 255.F, 255.F / 255.F}; 56 | 57 | class PathVisualizer { 58 | public: 59 | PathVisualizer() = default; 60 | virtual ~PathVisualizer() = default; 61 | 62 | void Initialize(const std::string& name, double vehicle_length, 63 | double vehicle_width); 64 | void Visualize(const std::vector& path); 65 | 66 | private: 67 | void Clear(); 68 | void UpdatePath(const std::vector& path); 69 | void AddSegment(const geometry_msgs::PoseStamped& pose_stamped); 70 | void AddNode(const geometry_msgs::PoseStamped& pose_stamped, 71 | std::size_t index); 72 | void AddVehicle(const geometry_msgs::PoseStamped& pose_stamped, 73 | std::size_t index); 74 | void PublishPath() const; 75 | void PublishPathNodes() const; 76 | void PublishPathVehicles() const; 77 | 78 | private: 79 | ros::Publisher path_pub_; 80 | ros::Publisher path_nodes_pub_; 81 | ros::Publisher path_vehicles_pub_; 82 | nav_msgs::Path path_; 83 | visualization_msgs::MarkerArray path_nodes_; 84 | visualization_msgs::MarkerArray path_vehicles_; 85 | std::string name_; 86 | double vehicle_length_ = 0.0; 87 | double vehicle_width_ = 0.0; 88 | bool initialized_ = false; 89 | }; 90 | 91 | } // namespace lattice_path_planner 92 | -------------------------------------------------------------------------------- /lattice_path_planner/lattice_a_star/lattice_a_star.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #pragma once 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include "lattice_path_planner/common/utils.h" 40 | #include "lattice_path_planner/grid_search/grid_search.h" 41 | #include "lattice_path_planner/lattice_a_star/node3d.h" 42 | #include "lattice_path_planner/primitive_generator/primitive_generator.h" 43 | 44 | namespace lattice_path_planner { 45 | namespace lattice_a_star { 46 | 47 | struct LatticeAStarResult { 48 | std::vector x; 49 | std::vector y; 50 | std::vector phi; 51 | std::vector v; 52 | std::vector a; 53 | std::vector steer; 54 | std::vector accumulated_s; 55 | }; 56 | 57 | class LatticeAStar { 58 | using Node3dPtr = std::unique_ptr; 59 | 60 | public: 61 | LatticeAStar() = default; 62 | virtual ~LatticeAStar(); 63 | void Init(int max_grid_x, int max_grid_y, double xy_grid_resolution, 64 | uint8_t obsthresh, uint8_t cost_inscribed_thresh, 65 | int cost_possibly_circumscribed_thresh, double nominalvel_mpersecs, 66 | double timetoturn45degsinplace_secs, 67 | const std::vector& footprint, 68 | char* motPrimFilename); 69 | bool Plan(double start_x, double start_y, double start_phi, double end_x, 70 | double end_y, double end_phi, 71 | std::vector>&& grid_map, 72 | LatticeAStarResult* result); 73 | 74 | private: 75 | bool SetStart(double start_x, double start_y, double start_phi); 76 | bool SetEnd(double end_x, double end_y, double end_phi); 77 | void LoadLatticeAStarResult(LatticeAStarResult* result); 78 | void Clear(); 79 | int CalcGridXYIndex(int grid_x, int grid_y) const; 80 | int CalcHeuCost(int grid_x, int grid_y) const; 81 | double EuclidHeuCost(int x1, int y1, int x2, int y2) const; 82 | bool ValidityCheck(const Node3d* node) const; 83 | bool IsValidCell(int grid_x, int grid_y) const; 84 | bool WorldToGrid(double x, double y, double phi, int* grid_x, int* grid_y, 85 | int* grid_phi) const; 86 | bool GridToWorld(int grid_x, int grid_y, int grid_phi, double* x, double* y, 87 | double* phi) const; 88 | bool IsWithinMap(int grid_x, int grid_y) const; 89 | void UpdateSuccs(const Node3d* curr_node); 90 | int GetActionCost(int curr_x, int curr_y, 91 | const primitive_generator::Primitive& action) const; 92 | Node3d* GetNode(int grid_x, int grid_y, int grid_phi); 93 | 94 | std::unique_ptr open_list_ = nullptr; 95 | std::vector> lattice_lookup_table_; 96 | std::vector> closed_list_; 97 | std::unique_ptr grid_a_star_heuristic_generator_ = 98 | nullptr; 99 | std::unique_ptr 100 | motion_primitive_generator_ = nullptr; 101 | common::EnvironmentConfig env_cfg_; 102 | 103 | Node3d* start_node_ = nullptr; 104 | Node3d* end_node_ = nullptr; 105 | std::size_t iterations_ = 0U; 106 | std::size_t created_node_num_ = 0U; 107 | bool initialized_ = false; 108 | }; 109 | 110 | } // namespace lattice_a_star 111 | } // namespace lattice_path_planner 112 | -------------------------------------------------------------------------------- /lattice_path_planner/common/heap.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #include "lattice_path_planner/common/heap.h" 33 | 34 | #include 35 | 36 | #include "glog/logging.h" 37 | 38 | namespace lattice_path_planner { 39 | namespace common { 40 | 41 | Heap::Heap() { queue_.resize(capacity_); } 42 | 43 | Heap::Heap(const std::size_t capacity) : capacity_(capacity) { 44 | queue_.resize(capacity); 45 | } 46 | 47 | Heap::~Heap() { Clear(); } 48 | 49 | void Heap::Clear() { 50 | for (std::size_t i = 1; i <= size_; ++i) { 51 | queue_[i].node->set_heap_index(0); 52 | } 53 | size_ = 0; 54 | } 55 | 56 | void Heap::PercolateDown(std::size_t hole, HeapElement obj) { 57 | // Sanity checks. 58 | CHECK(!Empty()); 59 | 60 | std::size_t child; 61 | for (; 2 * hole <= size_; hole = child) { 62 | child = 2 * hole; 63 | if (child != size_ && queue_[child + 1].key < queue_[child].key) { 64 | ++child; 65 | } 66 | if (queue_[child].key < obj.key) { 67 | queue_[hole] = queue_[child]; 68 | queue_[hole].node->set_heap_index(hole); 69 | } else { 70 | break; 71 | } 72 | } 73 | queue_[hole] = obj; 74 | queue_[hole].node->set_heap_index(hole); 75 | } 76 | 77 | void Heap::PercolateUp(std::size_t hole, HeapElement obj) { 78 | // Sanity checks. 79 | CHECK(!Empty()); 80 | 81 | for (; hole > 1 && obj.key < queue_[hole / 2].key; hole /= 2) { 82 | queue_[hole] = queue_[hole / 2]; 83 | queue_[hole].node->set_heap_index(hole); 84 | } 85 | queue_[hole] = obj; 86 | queue_[hole].node->set_heap_index(hole); 87 | } 88 | 89 | void Heap::PercolateUpOrDown(std::size_t hole, HeapElement obj) { 90 | // Sanity checks. 91 | CHECK(!Empty()); 92 | 93 | if (hole > 1 && obj.key < queue_[hole / 2].key) { 94 | PercolateUp(hole, obj); 95 | } else { 96 | PercolateDown(hole, obj); 97 | } 98 | } 99 | 100 | void Heap::Allocate() { 101 | std::stringstream ss; 102 | ss << "Growing heap size from " << capacity_ << " to "; 103 | capacity_ *= 2; 104 | queue_.resize(capacity_); 105 | VLOG(4) << ss.str() << capacity_; 106 | } 107 | 108 | void Heap::Insert(Node* node, int key) { 109 | // Sanity checks. 110 | if (node->heap_index() != 0) { 111 | LOG(ERROR) << "The node is already in the heap"; 112 | return; 113 | } 114 | 115 | if (size_ + 1 == capacity_) { 116 | Allocate(); 117 | } 118 | 119 | HeapElement obj; 120 | obj.node = node; 121 | obj.key = key; 122 | ++size_; 123 | PercolateUp(size_, obj); 124 | } 125 | 126 | Node* Heap::Pop() { 127 | // Sanity checks. 128 | if (Empty()) { 129 | LOG(ERROR) << "The heap is empty"; 130 | return nullptr; 131 | } 132 | 133 | Node* obj = queue_[1].node; 134 | obj->set_heap_index(0); 135 | if (size_ > 1) { 136 | PercolateDown(1, queue_[size_]); 137 | } 138 | --size_; 139 | return obj; 140 | } 141 | 142 | void Heap::Update(Node* node, int new_key) { 143 | // Sanity checks. 144 | if (node->heap_index() == 0) { 145 | LOG(ERROR) << "The node is not in the heap"; 146 | return; 147 | } 148 | 149 | if (queue_[node->heap_index()].key != new_key) { 150 | queue_[node->heap_index()].key = new_key; 151 | PercolateUpOrDown(node->heap_index(), queue_[node->heap_index()]); 152 | } 153 | } 154 | 155 | } // namespace common 156 | } // namespace lattice_path_planner 157 | -------------------------------------------------------------------------------- /lattice_path_planner/lattice_path_planner_ros.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2023, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | * Author: Jian Wen (nkuwenjian@gmail.com) 32 | *****************************************************************************/ 33 | 34 | #pragma once 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | #include "costmap_2d/costmap_2d_ros.h" 41 | #include "geometry_msgs/PoseStamped.h" 42 | #include "nav_core/base_global_planner.h" 43 | #include "ros/ros.h" 44 | 45 | #include "lattice_path_planner/lattice_a_star/lattice_a_star.h" 46 | #include "lattice_path_planner/path_visualizer.h" 47 | 48 | namespace lattice_path_planner { 49 | 50 | class LatticePathPlannerROS : public nav_core::BaseGlobalPlanner { 51 | public: 52 | /** 53 | * @brief Default constructor for the LatticePathPlannerROS object 54 | */ 55 | LatticePathPlannerROS() = default; 56 | 57 | /** 58 | * @brief Initialization function for the LatticePathPlannerROS object 59 | * @param name The name of this planner 60 | * @param costmap_ros A pointer to the ROS wrapper of the costmap to use 61 | */ 62 | void initialize(std::string name, 63 | costmap_2d::Costmap2DROS* costmap_ros) override; 64 | 65 | /** 66 | * @brief Given a goal pose in the world, compute a plan 67 | * @param start The start pose 68 | * @param goal The goal pose 69 | * @param plan The plan... filled by the planner 70 | * @return True if a valid plan was found, false otherwise 71 | */ 72 | bool makePlan(const geometry_msgs::PoseStamped& start, 73 | const geometry_msgs::PoseStamped& goal, 74 | std::vector& plan) override; 75 | 76 | ~LatticePathPlannerROS() override = default; 77 | 78 | private: 79 | bool UpdateCostmap(costmap_2d::Costmap2DROS* costmap_ros); 80 | 81 | bool LoadRosParamFromNodeHandle(const ros::NodeHandle& nh, 82 | double* nominalvel_mpersecs, 83 | double* timetoturn45degsinplace_secs, 84 | double* vehicle_length, 85 | double* vehicle_width); 86 | 87 | uint8_t ComputeCircumscribedCost() const; 88 | 89 | static std::vector GetFootprint( 90 | const std::vector& robot_footprint); 91 | 92 | static void GetStartAndEndConfigurations( 93 | const geometry_msgs::PoseStamped& start, 94 | const geometry_msgs::PoseStamped& goal, double origin_x, double origin_y, 95 | double* start_x, double* start_y, double* start_phi, double* end_x, 96 | double* end_y, double* end_phi); 97 | 98 | static std::vector> GetGridMap( 99 | const uint8_t* char_map, unsigned int size_x, unsigned int size_y, 100 | bool treat_unknown_as_free); 101 | 102 | static void PopulateGlobalPlan( 103 | const lattice_a_star::LatticeAStarResult& searched_path, 104 | const std_msgs::Header& header, double origin_x, double origin_y, 105 | std::vector* plan); 106 | 107 | std::unique_ptr planner_ = nullptr; 108 | std::unique_ptr visualizer_ = nullptr; 109 | bool initialized_ = false; 110 | std::string primitive_filename_; 111 | bool treat_unknown_as_free_ = false; 112 | 113 | std::string name_; 114 | const costmap_2d::Costmap2DROS* costmap_ros_ = nullptr; 115 | const costmap_2d::Costmap2D* costmap_2d_ = nullptr; 116 | costmap_2d::LayeredCostmap* layered_costmap_ = nullptr; 117 | }; 118 | 119 | } // namespace lattice_path_planner 120 | -------------------------------------------------------------------------------- /lattice_path_planner/grid_search/grid_search.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #pragma once 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include "glog/logging.h" 41 | 42 | #include "lattice_path_planner/common/constants.h" 43 | #include "lattice_path_planner/common/heap.h" 44 | #include "lattice_path_planner/grid_search/node2d.h" 45 | 46 | namespace lattice_path_planner { 47 | namespace grid_search { 48 | 49 | struct GridAStarResult { 50 | std::vector x; 51 | std::vector y; 52 | int path_cost = 0; 53 | }; 54 | 55 | struct GridSearchPrimitives { 56 | std::array dx; 57 | std::array dy; 58 | // the intermediate cells through which the actions go 59 | // for all the horizontal moving actions, they go to direct neighbors, so 60 | // initialize these intermediate cells to 0 61 | std::array dx0intersects; 62 | std::array dy0intersects; 63 | std::array dx1intersects; 64 | std::array dy1intersects; 65 | // distances of transitions 66 | std::array dxy_distance_mm; 67 | }; 68 | 69 | class GridSearch { 70 | public: 71 | enum class TerminationCondition { 72 | kOptPath, 73 | kTwentyPercentOverOptPath, 74 | kTwoTimesOptPath, 75 | kThreeTimesOptPath, 76 | kAllCells 77 | }; 78 | 79 | GridSearch() = default; 80 | virtual ~GridSearch(); 81 | 82 | void Init(int max_grid_x, int max_grid_y, double xy_grid_resolution, 83 | uint8_t obsthresh, TerminationCondition termination_condition); 84 | bool GenerateGridPath(int sx, int sy, int ex, int ey, 85 | const std::vector>& grid_map, 86 | GridAStarResult* result); 87 | int CheckDpMap(int grid_x, int grid_y); 88 | 89 | private: 90 | bool SetStart(int start_x, int start_y); 91 | bool SetEnd(int end_x, int end_y); 92 | bool SetStartAndEndConfiguration(int sx, int sy, int ex, int ey); 93 | Node2d* GetNode(int grid_x, int grid_y); 94 | int CalcHeuCost(int grid_x, int grid_y) const; 95 | bool IsWithinMap(int grid_x, int grid_y) const; 96 | bool IsValidCell(int grid_x, int grid_y) const; 97 | int CalcGridXYIndex(int grid_x, int grid_y) const; 98 | int GetKey(const Node2d* node) const; 99 | void UpdateSuccs(const Node2d* curr_node); 100 | void ComputeGridSearchActions(); 101 | int GetActionCost(int curr_x, int curr_y, int action_id) const; 102 | void LoadGridAStarResult(GridAStarResult* result) const; 103 | void Clear(); 104 | static float GetTerminationFactor(TerminationCondition termination_condition); 105 | 106 | int max_grid_x_ = 0; 107 | int max_grid_y_ = 0; 108 | double xy_grid_resolution_ = 0.0; 109 | std::vector> grid_map_; 110 | uint8_t obsthresh_; 111 | Node2d* start_node_ = nullptr; 112 | Node2d* end_node_ = nullptr; 113 | TerminationCondition termination_condition_; 114 | 115 | std::vector> dp_lookup_table_; 116 | std::unique_ptr open_list_ = nullptr; 117 | std::vector closed_list_; 118 | std::size_t iterations_ = 0U; 119 | 120 | GridSearchPrimitives actions_; 121 | bool initialized_ = false; 122 | }; 123 | 124 | } // namespace grid_search 125 | } // namespace lattice_path_planner 126 | -------------------------------------------------------------------------------- /lattice_path_planner/tf_broadcaster.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2023, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | * Author: Jian Wen (nkuwenjian@gmail.com) 32 | *****************************************************************************/ 33 | 34 | #include 35 | #include // NOLINT 36 | #include // NOLINT 37 | 38 | #include "geometry_msgs/PoseWithCovarianceStamped.h" 39 | #include "glog/logging.h" 40 | #include "ros/ros.h" 41 | #include "tf/tf.h" 42 | #include "tf/transform_broadcaster.h" 43 | 44 | namespace lattice_path_planner { 45 | 46 | class TfBroadcaster { 47 | public: 48 | TfBroadcaster() = default; 49 | virtual ~TfBroadcaster(); 50 | 51 | void Initialize(); 52 | 53 | private: 54 | void LoadRosParamFromNodeHandle(const ros::NodeHandle& nh, 55 | double* transform_publish_period); 56 | void SetStart( 57 | const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& start); 58 | void PublishTransform(); 59 | void PublishLoop(double transform_publish_period); 60 | 61 | ros::NodeHandle nh_; 62 | ros::Subscriber start_sub_; 63 | 64 | geometry_msgs::PoseStamped start_; 65 | bool start_received_ = false; 66 | std::mutex mutex_; 67 | std::unique_ptr tf_broadcaster_ = nullptr; 68 | std::unique_ptr transform_thread_ = nullptr; 69 | }; 70 | 71 | TfBroadcaster::~TfBroadcaster() { 72 | if (transform_thread_ != nullptr) { 73 | transform_thread_->join(); 74 | } 75 | } 76 | 77 | void TfBroadcaster::Initialize() { 78 | start_sub_ = nh_.subscribe("initialpose", 1, &TfBroadcaster::SetStart, this); 79 | 80 | tf_broadcaster_ = std::make_unique(); 81 | 82 | // Retrieve parameters 83 | ros::NodeHandle private_nh("~"); 84 | double transform_publish_period; 85 | LoadRosParamFromNodeHandle(private_nh, &transform_publish_period); 86 | 87 | // Create a thread to periodically publish the latest map->base_link 88 | // transform; it needs to go out regularly, uninterrupted by potentially 89 | // long periods of computation in our main loop. 90 | transform_thread_ = 91 | std::make_unique([this, transform_publish_period] { 92 | PublishLoop(transform_publish_period); 93 | }); 94 | } 95 | 96 | void TfBroadcaster::LoadRosParamFromNodeHandle( 97 | const ros::NodeHandle& nh, double* transform_publish_period) { 98 | nh.param("transform_publish_period", *transform_publish_period, 0.05); 99 | VLOG(4) << std::fixed 100 | << "transform_publish_period: " << *transform_publish_period; 101 | } 102 | 103 | void TfBroadcaster::PublishTransform() { 104 | std::lock_guard lock(mutex_); 105 | 106 | tf::Transform map_to_base_link; 107 | if (start_received_) { 108 | map_to_base_link = tf::Transform( 109 | tf::createQuaternionFromYaw(tf::getYaw(start_.pose.orientation)), 110 | tf::Point(start_.pose.position.x, start_.pose.position.y, 0.0)); 111 | } else { 112 | map_to_base_link.setIdentity(); 113 | } 114 | 115 | ros::Time tf_expiration = ros::Time::now(); 116 | tf_broadcaster_->sendTransform(tf::StampedTransform( 117 | map_to_base_link, tf_expiration, "map", "base_link")); 118 | } 119 | 120 | void TfBroadcaster::PublishLoop(double transform_publish_period) { 121 | if (transform_publish_period <= 0.0) { 122 | return; 123 | } 124 | 125 | ros::Rate r(1.0 / transform_publish_period); 126 | while (ros::ok()) { 127 | PublishTransform(); 128 | r.sleep(); 129 | } 130 | } 131 | 132 | void TfBroadcaster::SetStart( 133 | const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& start) { 134 | std::lock_guard lock(mutex_); 135 | 136 | LOG(INFO) << "A new start is received."; 137 | start_.header = start->header; 138 | start_.pose = start->pose.pose; 139 | start_received_ = true; 140 | } 141 | 142 | } // namespace lattice_path_planner 143 | 144 | int main(int argc, char* argv[]) { 145 | ros::init(argc, argv, "tf_broadcaster"); 146 | google::InitGoogleLogging(argv[0]); 147 | FLAGS_logtostderr = true; 148 | 149 | lattice_path_planner::TfBroadcaster tf_broadcaster; 150 | tf_broadcaster.Initialize(); 151 | ros::spin(); 152 | 153 | return 0; 154 | } 155 | -------------------------------------------------------------------------------- /lattice_path_planner/common/breadth_first_search.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #include "lattice_path_planner/common/breadth_first_search.h" 33 | 34 | #include 35 | 36 | namespace lattice_path_planner { 37 | namespace common { 38 | 39 | BreadthFirstSearch::BreadthFirstSearch(int size_x, int size_y, int obs_thresh) 40 | : size_x_(size_x), size_y_(size_y), obs_thresh_(obs_thresh) { 41 | // initialize the distance grid 42 | dist_.resize(size_x_); 43 | for (int x = 0; x < size_x_; x++) { 44 | dist_[x].resize(size_y_); 45 | } 46 | 47 | // initialize the actions 48 | dx_.resize(num_of_actions_); 49 | dy_.resize(num_of_actions_); 50 | int idx = 0; 51 | for (int x = -1; x <= 1; x++) { 52 | for (int y = -1; y <= 1; y++) { 53 | if (x == 0 && y == 0) { 54 | continue; 55 | } 56 | dx_[idx] = x; 57 | dy_[idx] = y; 58 | idx++; 59 | } 60 | } 61 | } 62 | 63 | bool BreadthFirstSearch::ComputeDistanceFromPoint( 64 | const std::vector>& grid, int x, int y) { 65 | if (x < 0 || x >= size_x_ || y < 0 || y >= size_y_) { 66 | LOG(ERROR) << "point is out of bounds!"; 67 | return false; 68 | } 69 | 70 | ClearDistances(); 71 | 72 | queue_.emplace(x, y); 73 | dist_[x][y] = 0; 74 | 75 | ComputeDistances(grid); 76 | return true; 77 | } 78 | 79 | bool BreadthFirstSearch::ComputeDistanceFromPoints( 80 | const std::vector>& grid, const std::vector& x, 81 | const std::vector& y) { 82 | if (x.size() != y.size()) { 83 | LOG(ERROR) << "size of x and y coordinates must agree!"; 84 | return false; 85 | } 86 | 87 | ClearDistances(); 88 | 89 | for (std::size_t i = 0U; i < x.size(); ++i) { 90 | if (x[i] < 0 || x[i] >= size_x_ || y[i] < 0 || y[i] >= size_y_) { 91 | LOG(ERROR) << "point is out of bounds!"; 92 | return false; 93 | } 94 | queue_.emplace(x[i], y[i]); 95 | dist_[x[i]][y[i]] = 0; 96 | } 97 | 98 | ComputeDistances(grid); 99 | return true; 100 | } 101 | 102 | void BreadthFirstSearch::ComputeDistanceFromObstacles( 103 | const std::vector>& grid) { 104 | ClearDistances(); 105 | 106 | for (int x = 0; x < size_x_; x++) { 107 | for (int y = 0; y < size_y_; y++) { 108 | if (grid[x][y] >= obs_thresh_) { 109 | queue_.emplace(x, y); 110 | dist_[x][y] = 0; 111 | } 112 | } 113 | } 114 | 115 | ComputeDistances(grid); 116 | } 117 | 118 | void BreadthFirstSearch::ComputeDistances( 119 | const std::vector>& grid) { 120 | XYCell cell; 121 | while (!queue_.empty()) { 122 | cell = queue_.front(); 123 | queue_.pop(); 124 | int cost = dist_[cell.x()][cell.y()] + 1; 125 | if (cell.x() == 0 || cell.x() == size_x_ - 1 || cell.y() == 0 || 126 | cell.y() == size_y_ - 1) { 127 | // we are on a boundary so we have to bounds check each successor 128 | for (std::size_t i = 0U; i < num_of_actions_; ++i) { 129 | int x = cell.x() + dx_[i]; 130 | int y = cell.y() + dy_[i]; 131 | if (x < 0 || x >= size_x_ || y < 0 || y >= size_y_) { 132 | continue; 133 | } 134 | if (dist_[x][y] < 0 && grid[x][y] < obs_thresh_) { 135 | dist_[x][y] = cost; 136 | queue_.emplace(x, y); 137 | } 138 | } 139 | } else { 140 | // we are not near a boundary so no bounds check is required 141 | for (std::size_t i = 0U; i < num_of_actions_; ++i) { 142 | int x = cell.x() + dx_[i]; 143 | int y = cell.y() + dy_[i]; 144 | if (dist_[x][y] < 0 && grid[x][y] < obs_thresh_) { 145 | dist_[x][y] = cost; 146 | queue_.emplace(x, y); 147 | } 148 | } 149 | } 150 | } 151 | } 152 | 153 | void BreadthFirstSearch::ClearDistances() { 154 | std::queue empty_queue; 155 | std::swap(queue_, empty_queue); 156 | 157 | for (int x = 0; x < size_x_; x++) { 158 | for (int y = 0; y < size_y_; y++) { 159 | dist_[x][y] = -1; 160 | } 161 | } 162 | } 163 | 164 | } // namespace common 165 | } // namespace lattice_path_planner 166 | -------------------------------------------------------------------------------- /lattice_path_planner/lattice_path_planner_ros_test.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2023, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | * Author: Jian Wen (nkuwenjian@gmail.com) 32 | *****************************************************************************/ 33 | 34 | #include "lattice_path_planner/lattice_path_planner_ros.h" 35 | 36 | #include 37 | #include // NOLINT 38 | 39 | #include "costmap_2d/costmap_2d_ros.h" 40 | #include "geometry_msgs/PoseStamped.h" 41 | #include "geometry_msgs/PoseWithCovarianceStamped.h" 42 | #include "glog/logging.h" 43 | #include "nav_msgs/Path.h" 44 | #include "ros/ros.h" 45 | #include "tf/tf.h" 46 | #include "tf2_ros/transform_listener.h" 47 | 48 | namespace lattice_path_planner { 49 | 50 | class LatticePathPlannerROSTest { 51 | public: 52 | explicit LatticePathPlannerROSTest(tf2_ros::Buffer& tf); // NOLINT 53 | virtual ~LatticePathPlannerROSTest() = default; 54 | 55 | void Initialize(); 56 | 57 | private: 58 | void SetStart( 59 | const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& start); 60 | void SetGoal(const geometry_msgs::PoseStamped::ConstPtr& goal); 61 | void MakePlan(); 62 | 63 | ros::NodeHandle nh_; 64 | ros::Subscriber start_sub_; 65 | ros::Subscriber goal_sub_; 66 | 67 | geometry_msgs::PoseStamped start_; 68 | geometry_msgs::PoseStamped goal_; 69 | bool start_received_ = false; 70 | bool goal_received_ = false; 71 | tf2_ros::Buffer& tf_; 72 | std::mutex start_mutex_; 73 | std::mutex goal_mutex_; 74 | 75 | std::unique_ptr planner_ = nullptr; 76 | std::unique_ptr costmap_ros_ = nullptr; 77 | }; 78 | 79 | LatticePathPlannerROSTest::LatticePathPlannerROSTest(tf2_ros::Buffer& tf) 80 | : tf_(tf) {} 81 | 82 | void LatticePathPlannerROSTest::Initialize() { 83 | start_sub_ = nh_.subscribe("initialpose", 1, 84 | &LatticePathPlannerROSTest::SetStart, this); 85 | goal_sub_ = nh_.subscribe("move_base_simple/goal", 1, 86 | &LatticePathPlannerROSTest::SetGoal, this); 87 | 88 | costmap_ros_ = 89 | std::make_unique("global_costmap", tf_); 90 | costmap_ros_->pause(); 91 | planner_ = std::make_unique(); 92 | planner_->initialize("LatticePathPlannerROS", costmap_ros_.get()); 93 | 94 | // Start actively updating costmaps based on sensor data. 95 | costmap_ros_->start(); 96 | } 97 | 98 | void LatticePathPlannerROSTest::SetStart( 99 | const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& start) { 100 | { 101 | std::lock_guard start_lock(start_mutex_); 102 | LOG(INFO) << "A new start is received."; 103 | start_.header = start->header; 104 | start_.pose = start->pose.pose; 105 | start_received_ = true; 106 | } 107 | 108 | MakePlan(); 109 | } 110 | 111 | void LatticePathPlannerROSTest::SetGoal( 112 | const geometry_msgs::PoseStamped::ConstPtr& goal) { 113 | { 114 | std::lock_guard goal_lock(goal_mutex_); 115 | LOG(INFO) << "A new goal is received."; 116 | goal_ = *goal; 117 | goal_received_ = true; 118 | } 119 | 120 | MakePlan(); 121 | } 122 | 123 | void LatticePathPlannerROSTest::MakePlan() { 124 | std::lock_guard start_lock(start_mutex_); 125 | std::lock_guard goal_lock(goal_mutex_); 126 | boost::unique_lock map_lock( 127 | *(costmap_ros_->getCostmap()->getMutex())); 128 | 129 | if (!start_received_ || !goal_received_) { 130 | return; 131 | } 132 | 133 | std::vector plan; 134 | if (planner_->makePlan(start_, goal_, plan)) { 135 | LOG(INFO) << "Successfully find a grid path via lattice A* algorithm."; 136 | } else { 137 | LOG(ERROR) << "Failed to find a grid path via lattice A* algorithm."; 138 | } 139 | } 140 | 141 | } // namespace lattice_path_planner 142 | 143 | int main(int argc, char* argv[]) { 144 | ros::init(argc, argv, "lattice_path_planner_test"); 145 | google::InitGoogleLogging(argv[0]); 146 | FLAGS_logtostderr = true; 147 | 148 | tf2_ros::Buffer buffer(ros::Duration(10)); 149 | tf2_ros::TransformListener tf(buffer); 150 | 151 | lattice_path_planner::LatticePathPlannerROSTest lattice_path_planner_test( 152 | buffer); 153 | lattice_path_planner_test.Initialize(); 154 | ros::spin(); 155 | 156 | return 0; 157 | } 158 | -------------------------------------------------------------------------------- /lattice_path_planner/path_visualizer.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #include "lattice_path_planner/path_visualizer.h" 33 | 34 | #include 35 | 36 | #include "glog/logging.h" 37 | 38 | namespace lattice_path_planner { 39 | 40 | void PathVisualizer::Initialize(const std::string& name, double vehicle_length, 41 | double vehicle_width) { 42 | if (initialized_) { 43 | LOG(INFO) << name << " path visualizer has been initialized."; 44 | return; 45 | } 46 | 47 | ros::NodeHandle private_nh("~/" + name); 48 | name_ = name; 49 | vehicle_length_ = vehicle_length; 50 | vehicle_width_ = vehicle_width; 51 | 52 | path_pub_ = private_nh.advertise("path", 1); 53 | path_nodes_pub_ = 54 | private_nh.advertise("path_nodes", 1); 55 | path_vehicles_pub_ = 56 | private_nh.advertise("path_vehicles", 1); 57 | 58 | LOG(INFO) << name << " path visualizer is initialized successfully."; 59 | initialized_ = true; 60 | } 61 | 62 | void PathVisualizer::Clear() { 63 | path_.poses.clear(); 64 | path_nodes_.markers.clear(); 65 | path_vehicles_.markers.clear(); 66 | } 67 | 68 | void PathVisualizer::Visualize( 69 | const std::vector& path) { 70 | if (!initialized_) { 71 | LOG(ERROR) << name_ << " path visualizer has not been initialized."; 72 | return; 73 | } 74 | 75 | Clear(); 76 | 77 | UpdatePath(path); 78 | 79 | PublishPath(); 80 | PublishPathNodes(); 81 | PublishPathVehicles(); 82 | } 83 | 84 | void PathVisualizer::UpdatePath( 85 | const std::vector& path) { 86 | path_.header.frame_id = "map"; 87 | path_.header.stamp = ros::Time::now(); 88 | 89 | std::size_t index = 0U; 90 | for (const geometry_msgs::PoseStamped& pose_stamped : path) { 91 | AddSegment(pose_stamped); 92 | AddNode(pose_stamped, index); 93 | ++index; 94 | AddVehicle(pose_stamped, index); 95 | ++index; 96 | } 97 | } 98 | 99 | void PathVisualizer::AddSegment( 100 | const geometry_msgs::PoseStamped& pose_stamped) { 101 | path_.poses.push_back(pose_stamped); 102 | } 103 | 104 | void PathVisualizer::AddNode(const geometry_msgs::PoseStamped& pose_stamped, 105 | std::size_t index) { 106 | visualization_msgs::Marker path_node; 107 | 108 | // Delete all previous markers. 109 | if (index == 0U) { 110 | path_node.action = visualization_msgs::Marker::DELETEALL; 111 | } 112 | 113 | path_node.header.frame_id = "map"; 114 | path_node.header.stamp = ros::Time(0); 115 | path_node.id = index; 116 | path_node.type = visualization_msgs::Marker::SPHERE; 117 | path_node.scale.x = 0.1; 118 | path_node.scale.y = 0.1; 119 | path_node.scale.z = 0.1; 120 | path_node.color.a = 1.0; 121 | 122 | // path_node.color.r = pink.red; 123 | // path_node.color.g = pink.green; 124 | // path_node.color.b = pink.blue; 125 | path_node.color.r = kPurple.red; 126 | path_node.color.g = kPurple.green; 127 | path_node.color.b = kPurple.blue; 128 | 129 | path_node.pose = pose_stamped.pose; 130 | path_nodes_.markers.push_back(std::move(path_node)); 131 | } 132 | 133 | void PathVisualizer::AddVehicle(const geometry_msgs::PoseStamped& pose_stamped, 134 | std::size_t index) { 135 | visualization_msgs::Marker path_vehicle; 136 | 137 | // Delete all previous markers. 138 | if (index == 1U) { 139 | path_vehicle.action = visualization_msgs::Marker::DELETEALL; 140 | } 141 | 142 | path_vehicle.header.frame_id = "map"; 143 | path_vehicle.header.stamp = ros::Time(0); 144 | path_vehicle.id = index; 145 | path_vehicle.type = visualization_msgs::Marker::CUBE; 146 | path_vehicle.scale.x = vehicle_length_; 147 | path_vehicle.scale.y = vehicle_width_; 148 | path_vehicle.scale.z = 1e-5; 149 | path_vehicle.color.a = 0.1; 150 | 151 | // path_vehicle.color.r = orange.red; 152 | // path_vehicle.color.g = orange.green; 153 | // path_vehicle.color.b = orange.blue; 154 | path_vehicle.color.r = kTeal.red; 155 | path_vehicle.color.g = kTeal.green; 156 | path_vehicle.color.b = kTeal.blue; 157 | 158 | path_vehicle.pose = pose_stamped.pose; 159 | path_vehicles_.markers.push_back(std::move(path_vehicle)); 160 | } 161 | 162 | void PathVisualizer::PublishPath() const { path_pub_.publish(path_); } 163 | 164 | void PathVisualizer::PublishPathNodes() const { 165 | path_nodes_pub_.publish(path_nodes_); 166 | } 167 | 168 | void PathVisualizer::PublishPathVehicles() const { 169 | path_vehicles_pub_.publish(path_vehicles_); 170 | } 171 | 172 | } // namespace lattice_path_planner 173 | -------------------------------------------------------------------------------- /launch/demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /LatticePathPlannerROS1 10 | Splitter Ratio: 0.5 11 | Tree Height: 719 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 0.699999988079071 56 | Class: rviz/Map 57 | Color Scheme: costmap 58 | Draw Behind: false 59 | Enabled: true 60 | Name: Map 61 | Topic: /lattice_path_planner_test/global_costmap/costmap 62 | Unreliable: false 63 | Use Timestamp: false 64 | Value: true 65 | - Class: rviz/Group 66 | Displays: 67 | - Alpha: 1 68 | Buffer Length: 1 69 | Class: rviz/Path 70 | Color: 25; 255; 0 71 | Enabled: true 72 | Head Diameter: 0.30000001192092896 73 | Head Length: 0.20000000298023224 74 | Length: 0.30000001192092896 75 | Line Style: Billboards 76 | Line Width: 0.10000000149011612 77 | Name: Path 78 | Offset: 79 | X: 0 80 | Y: 0 81 | Z: 0 82 | Pose Color: 255; 85; 255 83 | Pose Style: None 84 | Queue Size: 10 85 | Radius: 0.029999999329447746 86 | Shaft Diameter: 0.10000000149011612 87 | Shaft Length: 0.10000000149011612 88 | Topic: /lattice_path_planner_test/LatticePathPlannerROS/path 89 | Unreliable: false 90 | Value: true 91 | - Class: rviz/MarkerArray 92 | Enabled: true 93 | Marker Topic: /lattice_path_planner_test/LatticePathPlannerROS/path_nodes 94 | Name: PathNodes 95 | Namespaces: 96 | {} 97 | Queue Size: 100 98 | Value: true 99 | - Class: rviz/MarkerArray 100 | Enabled: true 101 | Marker Topic: /lattice_path_planner_test/LatticePathPlannerROS/path_vehicles 102 | Name: PathVehicles 103 | Namespaces: 104 | {} 105 | Queue Size: 100 106 | Value: true 107 | Enabled: true 108 | Name: LatticePathPlannerROS 109 | Enabled: true 110 | Global Options: 111 | Background Color: 255; 255; 255 112 | Default Light: true 113 | Fixed Frame: map 114 | Frame Rate: 30 115 | Name: root 116 | Tools: 117 | - Class: rviz/Interact 118 | Hide Inactive Objects: true 119 | - Class: rviz/MoveCamera 120 | - Class: rviz/Select 121 | - Class: rviz/FocusCamera 122 | - Class: rviz/Measure 123 | - Class: rviz/SetInitialPose 124 | Theta std deviation: 0.2617993950843811 125 | Topic: /initialpose 126 | X std deviation: 0.5 127 | Y std deviation: 0.5 128 | - Class: rviz/SetGoal 129 | Topic: /move_base_simple/goal 130 | - Class: rviz/PublishPoint 131 | Single click: true 132 | Topic: /clicked_point 133 | Value: true 134 | Views: 135 | Current: 136 | Class: rviz/Orbit 137 | Distance: 46.66357421875 138 | Enable Stereo Rendering: 139 | Stereo Eye Separation: 0.05999999865889549 140 | Stereo Focal Distance: 1 141 | Swap Stereo Eyes: false 142 | Value: false 143 | Field of View: 0.7853981852531433 144 | Focal Point: 145 | X: 1.6506736278533936 146 | Y: -0.1197604387998581 147 | Z: -0.00011656051356112584 148 | Focal Shape Fixed Size: true 149 | Focal Shape Size: 0.05000000074505806 150 | Invert Z Axis: false 151 | Name: Current View 152 | Near Clip Distance: 0.009999999776482582 153 | Pitch: 1.5697963237762451 154 | Target Frame: 155 | Yaw: 4.71238899230957 156 | Saved: ~ 157 | Window Geometry: 158 | Displays: 159 | collapsed: false 160 | Height: 1016 161 | Hide Left Dock: false 162 | Hide Right Dock: false 163 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c70000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 164 | Selection: 165 | collapsed: false 166 | Time: 167 | collapsed: false 168 | Tool Properties: 169 | collapsed: false 170 | Views: 171 | collapsed: false 172 | Width: 1848 173 | X: 72 174 | Y: 27 175 | -------------------------------------------------------------------------------- /lattice_path_planner/common/utils.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #include "lattice_path_planner/common/utils.h" 33 | 34 | #include 35 | 36 | #include "lattice_path_planner/common/breadth_first_search.h" 37 | 38 | namespace lattice_path_planner { 39 | namespace common { 40 | 41 | void ReadConfiguration(const std::string& file, int* max_grid_x, 42 | int* max_grid_y, uint8_t* obsthresh, 43 | uint8_t* cost_inscribed_thresh, 44 | int* cost_possibly_circumscribed_thresh, 45 | double* xy_grid_resolution, double* nominalvel_mpersecs, 46 | double* timetoturn45degsinplace_secs, 47 | std::vector>* grid_map) { 48 | std::ifstream fin(file, std::ios::in); 49 | if (!fin.is_open()) { 50 | return; 51 | } 52 | std::string line; 53 | std::vector strs; 54 | 55 | // discretization(cells): 56 | std::getline(fin, line); 57 | StrSplit(line, ' ', &strs); 58 | CHECK_EQ(strs.size(), 3); 59 | CHECK_EQ(strs[0], "discretization(cells):"); 60 | *max_grid_x = std::stoi(strs[1]); 61 | *max_grid_y = std::stoi(strs[2]); 62 | LOG(INFO) << "discretization(cells): " << *max_grid_x << " " << *max_grid_y; 63 | 64 | // obsthresh: 65 | std::getline(fin, line); 66 | StrSplit(line, ' ', &strs); 67 | CHECK_EQ(strs.size(), 2); 68 | CHECK_EQ(strs[0], "obsthresh:"); 69 | *obsthresh = static_cast(std::stoi(strs[1])); 70 | LOG(INFO) << "obsthresh: " << std::to_string(*obsthresh); 71 | 72 | // cost_inscribed_thresh: 73 | std::getline(fin, line); 74 | StrSplit(line, ' ', &strs); 75 | CHECK_EQ(strs.size(), 2); 76 | CHECK_EQ(strs[0], "cost_inscribed_thresh:"); 77 | *cost_inscribed_thresh = static_cast(std::stoi(strs[1])); 78 | LOG(INFO) << "cost_inscribed_thresh: " 79 | << std::to_string(*cost_inscribed_thresh); 80 | 81 | // cost_possibly_circumscribed_thresh: 82 | std::getline(fin, line); 83 | StrSplit(line, ' ', &strs); 84 | CHECK_EQ(strs.size(), 2); 85 | CHECK_EQ(strs[0], "cost_possibly_circumscribed_thresh:"); 86 | *cost_possibly_circumscribed_thresh = std::stoi(strs[1]); 87 | LOG(INFO) << "cost_possibly_circumscribed_thresh: " 88 | << *cost_possibly_circumscribed_thresh; 89 | 90 | // cellsize(meters): 91 | std::getline(fin, line); 92 | StrSplit(line, ' ', &strs); 93 | CHECK_EQ(strs.size(), 2); 94 | CHECK_EQ(strs[0], "cellsize(meters):"); 95 | *xy_grid_resolution = std::stod(strs[1]); 96 | LOG(INFO) << "cellsize(meters): " << *xy_grid_resolution; 97 | 98 | // nominalvel(mpersecs): 99 | std::getline(fin, line); 100 | StrSplit(line, ' ', &strs); 101 | CHECK_EQ(strs.size(), 2); 102 | CHECK_EQ(strs[0], "nominalvel(mpersecs):"); 103 | *nominalvel_mpersecs = std::stod(strs[1]); 104 | LOG(INFO) << "nominalvel(mpersecs): " << *nominalvel_mpersecs; 105 | 106 | // timetoturn45degsinplace(secs): 107 | std::getline(fin, line); 108 | StrSplit(line, ' ', &strs); 109 | CHECK_EQ(strs.size(), 2); 110 | CHECK_EQ(strs[0], "timetoturn45degsinplace(secs):"); 111 | *timetoturn45degsinplace_secs = std::stod(strs[1]); 112 | LOG(INFO) << "timetoturn45degsinplace(secs): " 113 | << *timetoturn45degsinplace_secs; 114 | 115 | // environment: 116 | std::getline(fin, line); 117 | CHECK_EQ(line, "environment:"); 118 | 119 | // allocate the 2D environment 120 | grid_map->resize(*max_grid_x); 121 | for (int x = 0; x < *max_grid_x; x++) { 122 | grid_map->at(x).resize(*max_grid_y); 123 | } 124 | 125 | std::size_t col = 0U; 126 | while (std::getline(fin, line)) { 127 | StrSplit(line, ' ', &strs); 128 | CHECK_EQ(strs.size(), *max_grid_x); 129 | for (int row = 0; row < *max_grid_x; row++) { 130 | grid_map->at(row)[col] = static_cast(std::stoi(strs[row])); 131 | } 132 | ++col; 133 | } 134 | CHECK_EQ(static_cast(col), *max_grid_y); 135 | fin.close(); 136 | } 137 | 138 | bool WriteConfiguration(const std::string& file, int max_grid_x, int max_grid_y, 139 | uint8_t obsthresh, uint8_t cost_inscribed_thresh, 140 | int cost_possibly_circumscribed_thresh, 141 | double xy_grid_resolution, double nominalvel_mpersecs, 142 | double timetoturn45degsinplace_secs, 143 | const std::vector>& grid_map) { 144 | std::ofstream fout; 145 | fout.open(file, std::ios::trunc); 146 | if (!fout.is_open()) { 147 | LOG(ERROR) << "Failed to open " << file; 148 | return false; 149 | } 150 | 151 | fout << "discretization(cells): " << max_grid_x << " " << max_grid_y << "\n"; 152 | fout << "obsthresh: " << std::to_string(obsthresh) << "\n"; 153 | fout << "cost_inscribed_thresh: " << std::to_string(cost_inscribed_thresh) 154 | << "\n"; 155 | fout << "cost_possibly_circumscribed_thresh: " 156 | << cost_possibly_circumscribed_thresh << "\n"; 157 | fout << std::fixed << "cellsize(meters): " << xy_grid_resolution << "\n"; 158 | fout << std::fixed << "nominalvel(mpersecs): " << nominalvel_mpersecs << "\n"; 159 | fout << std::fixed 160 | << "timetoturn45degsinplace(secs): " << timetoturn45degsinplace_secs 161 | << "\n"; 162 | fout << "environment:\n"; 163 | for (int y = 0; y < max_grid_y; y++) { 164 | for (int x = 0; x < max_grid_x; x++) { 165 | fout << std::to_string(grid_map[x][y]) << " "; 166 | } 167 | fout << "\n"; 168 | } 169 | 170 | fout.close(); 171 | return true; 172 | } 173 | 174 | } // namespace common 175 | } // namespace lattice_path_planner 176 | -------------------------------------------------------------------------------- /lattice_path_planner/common/footprint_helper.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #include "lattice_path_planner/common/footprint_helper.h" 33 | 34 | #include 35 | 36 | #include "lattice_path_planner/common/breadth_first_search.h" 37 | 38 | namespace lattice_path_planner { 39 | namespace common { 40 | 41 | // poses start at 0,0 continuous domain with half-bin less to account for 0,0 42 | // discrete domain start 43 | XYCellBounds FootprintHelper::GetMotionXYCells( 44 | const std::vector& polygon, const std::vector& poses, 45 | std::vector* cells, double res) { 46 | // the bounding box of the polygon 47 | XYCellBounds xybounds; 48 | 49 | // can't find any motion cells if there are no poses 50 | if (poses.empty()) { 51 | return xybounds; 52 | } 53 | 54 | CHECK_EQ(poses[0].x(), 0.0) << "poses should start at 0,0 continuous domain"; 55 | CHECK_EQ(poses[0].y(), 0.0) << "poses should start at 0,0 continuous domain"; 56 | 57 | XYThetaPoint tmp; 58 | XYPoint offset(DiscXY2Cont(0, res), DiscXY2Cont(0, res)); 59 | 60 | // get first footprint set 61 | std::set first_cell_set; 62 | tmp.set_x(poses[0].x() + offset.x()); 63 | tmp.set_y(poses[0].y() + offset.y()); 64 | tmp.set_theta(poses[0].theta()); 65 | xybounds = GetFootprintXYCells(polygon, &first_cell_set, tmp, res); 66 | 67 | // duplicate first footprint set into motion set 68 | std::set cell_set = first_cell_set; 69 | 70 | // call get footprint on the rest of the points 71 | for (std::size_t i = 1U; i < poses.size(); ++i) { 72 | tmp.set_x(poses[i].x() + offset.x()); 73 | tmp.set_y(poses[i].y() + offset.y()); 74 | tmp.set_theta(poses[i].theta()); 75 | xybounds.Add(GetFootprintXYCells(polygon, &cell_set, tmp, res)); 76 | } 77 | 78 | // convert the motion set to a vector but don't include the cells in the first 79 | // footprint set 80 | cells->reserve(cell_set.size() - first_cell_set.size()); 81 | for (const XYCell& cell : cell_set) { 82 | if (first_cell_set.find(cell) == first_cell_set.end()) { 83 | cells->push_back(cell); 84 | } 85 | } 86 | 87 | return xybounds; 88 | } 89 | 90 | // This function is inefficient and should be avoided if possible (you should 91 | // use overloaded functions that uses a set for the cells)! 92 | // pose w.r.t 0,0 discrete domain start 93 | void FootprintHelper::GetFootprintXYCells(const std::vector& polygon, 94 | std::vector* cells, 95 | const XYThetaPoint& pose, 96 | double res) { 97 | int x = ContXY2Disc(pose.x(), res); 98 | int y = ContXY2Disc(pose.y(), res); 99 | CHECK_EQ(pose.x(), DiscXY2Cont(x, res)) 100 | << "pose should be w.r.t 0,0 discrete domain start"; 101 | CHECK_EQ(pose.y(), DiscXY2Cont(y, res)) 102 | << "pose should be w.r.t 0,0 discrete domain start"; 103 | 104 | std::set cell_set; 105 | for (const XYCell& cell : *cells) { 106 | cell_set.insert(cell); 107 | } 108 | GetFootprintXYCells(polygon, &cell_set, pose, res); 109 | cells->clear(); 110 | cells->reserve(cell_set.size()); 111 | 112 | for (const XYCell& cell : cell_set) { 113 | cells->push_back(cell); 114 | } 115 | } 116 | 117 | // pose w.r.t 0,0 discrete domain start 118 | XYCellBounds FootprintHelper::GetFootprintXYCells( 119 | const std::vector& polygon, std::set* cells, 120 | const XYThetaPoint& pose, double res) { 121 | // the bounding box of the polygon 122 | XYCellBounds xybounds; 123 | 124 | // special case for point robot 125 | if (polygon.size() <= 1) { 126 | cells->insert({ContXY2Disc(pose.x(), res), ContXY2Disc(pose.y(), res)}); 127 | xybounds.set_minimum(XYCell()); 128 | xybounds.set_maximum(XYCell()); 129 | return xybounds; 130 | } 131 | 132 | XYPoint sourcepose(DiscXY2Cont(0, res), DiscXY2Cont(0, res)); 133 | 134 | // run bressenham line algorithm around the polygon (add them to the cells 135 | // set) while doing that find the min and max (x,y) and the average x and y 136 | double cth = std::cos(pose.theta()); 137 | double sth = std::sin(pose.theta()); 138 | 139 | std::vector disc_polygon; 140 | disc_polygon.reserve(polygon.size() + 1); 141 | int minx = std::numeric_limits::max(); 142 | int maxx = -std::numeric_limits::max(); 143 | int miny = std::numeric_limits::max(); 144 | int maxy = -std::numeric_limits::max(); 145 | 146 | // find the bounding box of the polygon 147 | for (const XYPoint& point : polygon) { 148 | XYCell p; 149 | p.set_x(ContXY2Disc(cth * point.x() - sth * point.y() + pose.x(), res)); 150 | p.set_y(ContXY2Disc(sth * point.x() + cth * point.y() + pose.y(), res)); 151 | disc_polygon.push_back(p); 152 | if (p.x() < minx) { 153 | minx = p.x(); 154 | } 155 | if (p.x() > maxx) { 156 | maxx = p.x(); 157 | } 158 | if (p.y() < miny) { 159 | miny = p.y(); 160 | } 161 | if (p.y() > maxy) { 162 | maxy = p.y(); 163 | } 164 | 165 | // compute the bounding box 166 | p.set_x( 167 | ContXY2Disc(cth * point.x() - sth * point.y() + sourcepose.x(), res)); 168 | p.set_y( 169 | ContXY2Disc(sth * point.x() + cth * point.y() + sourcepose.y(), res)); 170 | xybounds.Add(p); 171 | } 172 | disc_polygon.push_back(disc_polygon.front()); 173 | 174 | // make a grid big enough for the footprint 175 | int sizex = (maxx - minx + 1) + 2; 176 | int sizey = (maxy - miny + 1) + 2; 177 | std::vector> grid; 178 | grid.resize(sizex); 179 | for (int i = 0; i < sizex; ++i) { 180 | grid[i].resize(sizey); 181 | for (int j = 0; j < sizey; ++j) { 182 | grid[i][j] = 0; 183 | } 184 | } 185 | 186 | // plot line points on the grid 187 | for (std::size_t i = 1U; i < disc_polygon.size(); ++i) { 188 | int x0 = disc_polygon[i - 1].x() - minx + 1; 189 | int y0 = disc_polygon[i - 1].y() - miny + 1; 190 | int x1 = disc_polygon[i].x() - minx + 1; 191 | int y1 = disc_polygon[i].y() - miny + 1; 192 | 193 | // bressenham (add the line cells to the set and to a vector) 194 | bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); 195 | if (steep) { 196 | int temp = x0; 197 | x0 = y0; 198 | y0 = temp; 199 | temp = x1; 200 | x1 = y1; 201 | y1 = temp; 202 | } 203 | if (x0 > x1) { 204 | int temp = x0; 205 | x0 = x1; 206 | x1 = temp; 207 | temp = y0; 208 | y0 = y1; 209 | y1 = temp; 210 | } 211 | int deltax = x1 - x0; 212 | int deltay = std::abs(y1 - y0); 213 | int error = deltax / 2; 214 | int ystep = (y0 < y1 ? 1 : -1); 215 | int y = y0; 216 | for (int x = x0; x <= x1; x++) { 217 | if (steep) { 218 | grid[y][x] = 1; 219 | cells->insert({y - 1 + minx, x - 1 + miny}); 220 | } else { 221 | grid[x][y] = 1; 222 | cells->insert({x - 1 + minx, y - 1 + miny}); 223 | } 224 | int last_error = error; 225 | error -= deltay; 226 | if (error < 0 && x != x1) { 227 | // make sure we can't have a diagonal line (the 8-connected bfs will 228 | // leak through) 229 | 230 | int tempy = y; 231 | int tempx = x; 232 | if (last_error < -error) { 233 | tempy += ystep; 234 | } else { 235 | tempx += 1; 236 | } 237 | if (steep) { 238 | grid[tempy][tempx] = 1; 239 | cells->insert({tempy - 1 + minx, tempx - 1 + miny}); 240 | } else { 241 | grid[tempx][tempy] = 1; 242 | cells->insert({tempx - 1 + minx, tempy - 1 + miny}); 243 | } 244 | 245 | y += ystep; 246 | error += deltax; 247 | } 248 | } 249 | } 250 | 251 | // run a 2d bfs from the average (x,y) 252 | BreadthFirstSearch bfs(sizex, sizey, 1); 253 | bfs.ComputeDistanceFromPoint(grid, 0, 0); 254 | 255 | // add all cells expanded to the cells set 256 | for (int i = 1; i < sizex - 1; ++i) { 257 | for (int j = 1; j < sizey - 1; ++j) { 258 | if (bfs.GetDistance(i, j) < 0) { 259 | cells->insert({i - 1 + minx, j - 1 + miny}); 260 | } 261 | } 262 | } 263 | 264 | return xybounds; 265 | } 266 | 267 | } // namespace common 268 | } // namespace lattice_path_planner 269 | -------------------------------------------------------------------------------- /lattice_path_planner/common/utils.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #pragma once 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include "glog/logging.h" 41 | 42 | #include "lattice_path_planner/common/constants.h" 43 | 44 | namespace lattice_path_planner { 45 | namespace common { 46 | 47 | // input angle should be in radians 48 | // counterclockwise is positive 49 | // output is an angle in the range of from 0 to 2*PI 50 | inline double NormalizeAngle(double angle) { 51 | double retangle = angle; 52 | 53 | // get to the range from -2PI, 2PI 54 | if (std::abs(retangle) > 2 * M_PI) { 55 | retangle = retangle - static_cast(retangle / (2 * M_PI)) * 2 * M_PI; 56 | } 57 | 58 | // get to the range 0, 2PI 59 | if (retangle < 0) { 60 | retangle += 2 * M_PI; 61 | } 62 | 63 | CHECK(retangle >= 0 && retangle < 2 * M_PI) 64 | << "after normalization of angle=" << angle 65 | << " we get angle=" << retangle; 66 | return retangle; 67 | } 68 | 69 | inline int NormalizeDiscTheta(int nTheta, int THETADIRS) { 70 | return nTheta >= 0 ? nTheta % THETADIRS 71 | : (nTheta % THETADIRS + THETADIRS) % THETADIRS; 72 | } 73 | 74 | /** 75 | * \brief computes minimum unsigned difference between two angles in radians 76 | */ 77 | inline double ComputeMinUnsignedAngleDiff(double angle1, double angle2) { 78 | // get the angles into 0-2*PI range 79 | angle1 = NormalizeAngle(angle1); 80 | angle2 = NormalizeAngle(angle2); 81 | 82 | double anglediff = std::abs(angle1 - angle2); 83 | 84 | // see if we can take a shorter route 85 | if (anglediff > M_PI) { 86 | anglediff = std::abs(anglediff - 2 * M_PI); 87 | } 88 | 89 | return anglediff; 90 | } 91 | 92 | // converts discretized version of angle into continuous (radians) 93 | // maps 0->0, 1->delta, 2->2*delta, ... 94 | inline double DiscTheta2Cont(int nTheta, int NUMOFANGLEVALS) { 95 | double thetaBinSize = 2.0 * M_PI / NUMOFANGLEVALS; 96 | return nTheta * thetaBinSize; 97 | } 98 | 99 | // converts continuous (radians) version of angle into discrete 100 | // maps 0->0, [delta/2, 3/2*delta)->1, [3/2*delta, 5/2*delta)->2,... 101 | inline int ContTheta2Disc(double fTheta, int NUMOFANGLEVALS) { 102 | double thetaBinSize = 2.0 * M_PI / NUMOFANGLEVALS; 103 | return static_cast(NormalizeAngle(fTheta + thetaBinSize / 2.0) / 104 | (2.0 * M_PI) * (NUMOFANGLEVALS)); 105 | } 106 | 107 | inline double DiscXY2Cont(int X, double CELLSIZE) { 108 | return X * CELLSIZE + CELLSIZE / 2.0; 109 | } 110 | 111 | inline int ContXY2Disc(double X, double CELLSIZE) { 112 | return X >= 0.0 ? static_cast(X / CELLSIZE) 113 | : static_cast(X / CELLSIZE) - 1; 114 | } 115 | 116 | class XYCell { 117 | public: 118 | XYCell() = default; 119 | XYCell(int x, int y) : x_(x), y_(y) {} 120 | virtual ~XYCell() = default; 121 | 122 | void set_x(int x) { x_ = x; } 123 | void set_y(int y) { y_ = y; } 124 | int x() const { return x_; } 125 | int y() const { return y_; } 126 | int* mutable_x() { return &x_; } 127 | int* mutable_y() { return &y_; } 128 | 129 | void MakeFloor(const XYCell& cell) { 130 | if (cell.x_ < x_) { 131 | x_ = cell.x_; 132 | } 133 | if (cell.y_ < y_) { 134 | y_ = cell.y_; 135 | } 136 | } 137 | 138 | void MakeCeil(const XYCell& cell) { 139 | if (cell.x_ > x_) { 140 | x_ = cell.x_; 141 | } 142 | if (cell.y_ > y_) { 143 | y_ = cell.y_; 144 | } 145 | } 146 | 147 | bool operator==(const XYCell& rhs) const { 148 | return x_ == rhs.x_ && y_ == rhs.y_; 149 | } 150 | 151 | bool operator<(const XYCell& rhs) const { 152 | return x_ < rhs.x_ || (x_ == rhs.x_ && y_ < rhs.y_); 153 | } 154 | 155 | private: 156 | int x_ = 0; 157 | int y_ = 0; 158 | }; 159 | 160 | class XYPoint { 161 | public: 162 | XYPoint() = default; 163 | XYPoint(double x, double y) : x_(x), y_(y) {} 164 | virtual ~XYPoint() = default; 165 | 166 | void set_x(double x) { x_ = x; } 167 | void set_y(double y) { y_ = y; } 168 | double x() const { return x_; } 169 | double y() const { return y_; } 170 | double* mutable_x() { return &x_; } 171 | double* mutable_y() { return &y_; } 172 | 173 | private: 174 | double x_ = 0.0; 175 | double y_ = 0.0; 176 | }; 177 | 178 | class XYThetaPoint { 179 | public: 180 | XYThetaPoint() = default; 181 | XYThetaPoint(double x, double y, double theta) 182 | : x_(x), y_(y), theta_(theta) {} 183 | virtual ~XYThetaPoint() = default; 184 | 185 | void set_x(double x) { x_ = x; } 186 | void set_y(double y) { y_ = y; } 187 | void set_theta(double theta) { theta_ = theta; } 188 | double x() const { return x_; } 189 | double y() const { return y_; } 190 | double theta() const { return theta_; } 191 | double* mutable_x() { return &x_; } 192 | double* mutable_y() { return &y_; } 193 | double* mutable_theta() { return &theta_; } 194 | 195 | private: 196 | double x_ = 0.0; 197 | double y_ = 0.0; 198 | double theta_ = 0.0; 199 | }; 200 | 201 | // configuration parameters 202 | struct EnvironmentConfig { 203 | int max_grid_x; 204 | int max_grid_y; 205 | std::vector> grid_map; 206 | 207 | // the value at which and above which cells are obstacles in the maps sent 208 | // from outside the default is defined above 209 | uint8_t obsthresh; 210 | 211 | // the value at which and above which until obsthresh (not including it) 212 | // cells have the nearest obstacle at distance smaller than or equal to 213 | // the inner circle of the robot. In other words, the robot is definitely 214 | // colliding with the obstacle, independently of its orientation 215 | // if no such cost is known, then it should be set to obsthresh (if center 216 | // of the robot collides with obstacle, then the whole robot collides with 217 | // it independently of its rotation) 218 | uint8_t cost_inscribed_thresh; 219 | 220 | // the value at which and above which until cost_inscribed_thresh (not 221 | // including it) cells 222 | // **may** have a nearest osbtacle within the distance that is in between 223 | // the robot inner circle and the robot outer circle 224 | // any cost below this value means that the robot will NOT collide with any 225 | // obstacle, independently of its orientation 226 | // if no such cost is known, then it should be set to 0 or -1 (then no cell 227 | // cost will be lower than it, and therefore the robot's footprint will 228 | // always be checked) 229 | int cost_possibly_circumscribed_thresh; // it has to be integer, because -1 230 | // means that it is not provided. 231 | 232 | double nominalvel_mpersecs; 233 | 234 | double timetoturn45degsinplace_secs; 235 | 236 | double xy_grid_resolution; 237 | int phi_grid_resolution = kPhiGridResolution; 238 | 239 | std::vector footprint; 240 | }; 241 | 242 | /** 243 | * Defines a bounding box in 2-dimensional real space. 244 | */ 245 | class XYCellBounds { 246 | public: 247 | XYCellBounds() 248 | : minimum_(std::numeric_limits::max(), 249 | std::numeric_limits::max()), 250 | maximum_(-std::numeric_limits::max(), 251 | -std::numeric_limits::max()) {} 252 | 253 | virtual ~XYCellBounds() = default; 254 | 255 | const XYCell& minimum() const { return minimum_; } 256 | const XYCell& maximum() const { return maximum_; } 257 | 258 | void set_minimum(const XYCell& minimum) { minimum_ = minimum; } 259 | void set_maximum(const XYCell& maximum) { maximum_ = maximum; } 260 | 261 | void Add(const XYCell& rhs) { 262 | minimum_.MakeFloor(rhs); 263 | maximum_.MakeCeil(rhs); 264 | } 265 | 266 | void Add(const XYCellBounds& bounds) { 267 | Add(bounds.minimum_); 268 | Add(bounds.maximum_); 269 | } 270 | 271 | bool IsInBounds(const XYCell& rhs) const { 272 | return rhs.x() >= minimum_.x() && rhs.x() <= maximum_.x() && 273 | rhs.y() >= minimum_.y() && rhs.y() <= maximum_.y(); 274 | } 275 | 276 | private: 277 | XYCell minimum_; 278 | XYCell maximum_; 279 | }; 280 | 281 | inline void StrSplit(const std::string& str, char spliter, 282 | std::vector* out) { 283 | if (out == nullptr) { 284 | return; 285 | } 286 | out->clear(); 287 | std::size_t posBegin(0); 288 | std::size_t posFind = str.find(spliter, posBegin); 289 | while (posFind != std::string::npos) { 290 | std::string tmp = str.substr(posBegin, posFind - posBegin); 291 | out->push_back(tmp); 292 | posBegin = posFind + 1; 293 | posFind = str.find(spliter, posBegin); 294 | } 295 | 296 | std::string tmp = str.substr(posBegin, str.length() - posBegin); 297 | if (!tmp.empty()) { 298 | out->push_back(tmp); 299 | } 300 | } 301 | 302 | void ReadConfiguration(const std::string& file, int* max_grid_x, 303 | int* max_grid_y, uint8_t* obsthresh, 304 | uint8_t* cost_inscribed_thresh, 305 | int* cost_possibly_circumscribed_thresh, 306 | double* xy_grid_resolution, double* nominalvel_mpersecs, 307 | double* timetoturn45degsinplace_secs, 308 | std::vector>* grid_map); 309 | 310 | bool WriteConfiguration(const std::string& file, int max_grid_x, int max_grid_y, 311 | uint8_t obsthresh, uint8_t cost_inscribed_thresh, 312 | int cost_possibly_circumscribed_thresh, 313 | double xy_grid_resolution, double nominalvel_mpersecs, 314 | double timetoturn45degsinplace_secs, 315 | const std::vector>& grid_map); 316 | 317 | } // namespace common 318 | } // namespace lattice_path_planner 319 | -------------------------------------------------------------------------------- /lattice_path_planner/primitive_generator/primitive_generator.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #include "lattice_path_planner/primitive_generator/primitive_generator.h" 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include "lattice_path_planner/common/footprint_helper.h" 40 | #include "lattice_path_planner/common/utils.h" 41 | 42 | namespace lattice_path_planner { 43 | namespace primitive_generator { 44 | 45 | PrimitiveGenerator::PrimitiveGenerator(const common::EnvironmentConfig& env_cfg) 46 | : env_cfg_(env_cfg) {} 47 | 48 | void PrimitiveGenerator::Init(const std::string& file) { 49 | ReadMotionPrimitives(file); 50 | PrecomputeActionswithCompleteMotionPrimitive(); 51 | } 52 | 53 | bool PrimitiveGenerator::ReadMotionPrimitives(const std::string& file) { 54 | std::ifstream fin(file, std::ios::in); 55 | if (!fin.is_open()) { 56 | return false; 57 | } 58 | std::string line; 59 | std::vector strs; 60 | 61 | // resolution_m: 62 | std::getline(fin, line); 63 | common::StrSplit(line, ' ', &strs); 64 | CHECK_EQ(strs.size(), 2); 65 | CHECK_EQ(strs[0], "resolution_m:"); 66 | CHECK_NEAR(std::abs(std::stod(strs[1])), 67 | std::abs(env_cfg_.xy_grid_resolution), 1e-6); 68 | VLOG(4) << std::fixed << "resolution_m: " << std::stod(strs[1]); 69 | 70 | // numberofangles: 71 | std::getline(fin, line); 72 | common::StrSplit(line, ' ', &strs); 73 | CHECK_EQ(strs.size(), 2); 74 | CHECK_EQ(strs[0], "numberofangles:"); 75 | phi_grid_resolution_ = std::stoi(strs[1]); 76 | VLOG(4) << "numberofangles: " << strs[1]; 77 | 78 | // totalnumberofprimitives: 79 | std::getline(fin, line); 80 | common::StrSplit(line, ' ', &strs); 81 | CHECK_EQ(strs.size(), 2); 82 | CHECK_EQ(strs[0], "totalnumberofprimitives:"); 83 | int total_num_of_actions = std::stoi(strs[1]); 84 | VLOG(4) << "totalnumberofprimitives: " << total_num_of_actions; 85 | 86 | // Read in motion primitive for each action 87 | CHECK_EQ(total_num_of_actions % phi_grid_resolution_, 0); 88 | action_width_ = total_num_of_actions / phi_grid_resolution_; 89 | VLOG(4) << "action_width: " << action_width_; 90 | for (int tind = 0; tind < phi_grid_resolution_; tind++) { 91 | std::vector primitives; 92 | for (int mind = 0; mind < action_width_; mind++) { 93 | Primitive primitive; 94 | ReadinMotionPrimitive(&fin, &primitive); 95 | CHECK_EQ(static_cast(primitive.start_grid_phi), tind); 96 | primitives.push_back(std::move(primitive)); 97 | } 98 | motion_primitives_.push_back(std::move(primitives)); 99 | } 100 | 101 | fin.close(); 102 | return true; 103 | } 104 | 105 | void PrimitiveGenerator::ReadinMotionPrimitive(std::ifstream* fin, 106 | Primitive* prim) const { 107 | std::string line; 108 | std::vector strs; 109 | 110 | // primID: 111 | std::getline(*fin, line); 112 | common::StrSplit(line, ' ', &strs); 113 | CHECK_EQ(strs.size(), 2); 114 | CHECK_EQ(strs[0], "primID:"); 115 | prim->id = std::stoi(strs[1]); 116 | 117 | // startangle_c: 118 | std::getline(*fin, line); 119 | common::StrSplit(line, ' ', &strs); 120 | CHECK_EQ(strs.size(), 2); 121 | CHECK_EQ(strs[0], "startangle_c:"); 122 | prim->start_grid_phi = std::stoi(strs[1]); 123 | 124 | // endpose_c: 125 | std::getline(*fin, line); 126 | common::StrSplit(line, ' ', &strs); 127 | CHECK_EQ(strs.size(), 4); 128 | CHECK_EQ(strs[0], "endpose_c:"); 129 | prim->end_grid_x = std::stoi(strs[1]); 130 | prim->end_grid_y = std::stoi(strs[2]); 131 | prim->end_grid_phi = std::stoi(strs[3]); 132 | // normalize the angle 133 | prim->end_grid_phi = 134 | common::NormalizeDiscTheta(prim->end_grid_phi, phi_grid_resolution_); 135 | CHECK_GE(prim->end_grid_phi, 0); 136 | CHECK_LT(prim->end_grid_phi, phi_grid_resolution_); 137 | 138 | // additionalactioncostmult: 139 | std::getline(*fin, line); 140 | common::StrSplit(line, ' ', &strs); 141 | CHECK_EQ(strs.size(), 2); 142 | CHECK_EQ(strs[0], "additionalactioncostmult:"); 143 | prim->multiplier = std::stoi(strs[1]); 144 | 145 | // intermediateposes: 146 | std::getline(*fin, line); 147 | common::StrSplit(line, ' ', &strs); 148 | CHECK_EQ(strs.size(), 2); 149 | CHECK_EQ(strs[0], "intermediateposes:"); 150 | int num_of_interm_poses = std::stoi(strs[1]); 151 | 152 | // all intermposes should be with respect to 0,0 as starting pose since it 153 | // will be added later and should be done after the action is rotated by 154 | // initial orientation 155 | for (int i = 0; i < num_of_interm_poses; ++i) { 156 | common::XYThetaPoint pose; 157 | ReadinPose(fin, pose.mutable_x(), pose.mutable_y(), pose.mutable_theta()); 158 | prim->intermptV.emplace_back(std::move(pose)); 159 | } 160 | 161 | // Check that the last pose of the motion matches (within lattice 162 | // resolution) the designated end pose of the primitive 163 | double source_pose_x = common::DiscXY2Cont(0, env_cfg_.xy_grid_resolution); 164 | double source_pose_y = common::DiscXY2Cont(0, env_cfg_.xy_grid_resolution); 165 | double end_pose_x = source_pose_x + prim->intermptV.back().x(); 166 | double end_pose_y = source_pose_y + prim->intermptV.back().y(); 167 | double end_pose_phi = prim->intermptV.back().theta(); 168 | 169 | int end_grid_x = common::ContXY2Disc(end_pose_x, env_cfg_.xy_grid_resolution); 170 | int end_grid_y = common::ContXY2Disc(end_pose_y, env_cfg_.xy_grid_resolution); 171 | int end_grid_phi = common::ContTheta2Disc(end_pose_phi, phi_grid_resolution_); 172 | CHECK_EQ(end_grid_x, prim->end_grid_x); 173 | CHECK_EQ(end_grid_y, prim->end_grid_y); 174 | CHECK_EQ(end_grid_phi, prim->end_grid_phi); 175 | } 176 | 177 | void PrimitiveGenerator::ReadinPose(std::ifstream* fin, double* x, double* y, 178 | double* phi) { 179 | std::string line; 180 | std::vector strs; 181 | std::getline(*fin, line); 182 | common::StrSplit(line, ' ', &strs); 183 | CHECK_EQ(strs.size(), 3); 184 | 185 | *x = std::stod(strs[0]); 186 | *y = std::stod(strs[1]); 187 | *phi = std::stod(strs[2]); 188 | *phi = common::NormalizeAngle(*phi); 189 | } 190 | 191 | void PrimitiveGenerator::PrecomputeActionswithCompleteMotionPrimitive() { 192 | // compute sourcepose 193 | const double source_pose_x = 194 | common::DiscXY2Cont(0, env_cfg_.xy_grid_resolution); 195 | const double source_pose_y = 196 | common::DiscXY2Cont(0, env_cfg_.xy_grid_resolution); 197 | 198 | // iterate over source angles 199 | for (int tind = 0; tind < phi_grid_resolution_; tind++) { 200 | // iterate over motion primitives 201 | for (int mind = 0; mind < action_width_; mind++) { 202 | CHECK_EQ(motion_primitives_[tind][mind].id, mind); 203 | 204 | // start angle 205 | CHECK_EQ(static_cast(motion_primitives_[tind][mind].start_grid_phi), 206 | tind); 207 | 208 | // compute and store interm points as well as intersecting cells 209 | motion_primitives_[tind][mind].intersectingcellsV.clear(); 210 | motion_primitives_[tind][mind].interm2DcellsV.clear(); 211 | 212 | std::set interm2DcellSet; 213 | // Compute all the intersected cells for this action (intermptV and 214 | // interm3DcellsV) 215 | for (const common::XYThetaPoint& intermpt : 216 | motion_primitives_[tind][mind].intermptV) { 217 | // also compute the intermediate discrete cells if not there already 218 | double x = intermpt.x() + source_pose_x; 219 | double y = intermpt.y() + source_pose_y; 220 | 221 | // add unique cells to the list 222 | interm2DcellSet.insert( 223 | {common::ContXY2Disc(x, env_cfg_.xy_grid_resolution), 224 | common::ContXY2Disc(y, env_cfg_.xy_grid_resolution)}); 225 | } 226 | 227 | for (const common::XYCell& cell : interm2DcellSet) { 228 | motion_primitives_[tind][mind].interm2DcellsV.push_back(cell); 229 | } 230 | 231 | // compute linear and angular time 232 | double linear_distance = 0.0; 233 | for (std::size_t i = 1U; 234 | i < motion_primitives_[tind][mind].intermptV.size(); ++i) { 235 | double x0 = motion_primitives_[tind][mind].intermptV[i - 1].x(); 236 | double y0 = motion_primitives_[tind][mind].intermptV[i - 1].y(); 237 | double x1 = motion_primitives_[tind][mind].intermptV[i].x(); 238 | double y1 = motion_primitives_[tind][mind].intermptV[i].y(); 239 | linear_distance += std::hypot(x1 - x0, y1 - y0); 240 | } 241 | double linear_time = linear_distance / env_cfg_.nominalvel_mpersecs; 242 | double angular_distance = std::abs(common::ComputeMinUnsignedAngleDiff( 243 | common::DiscTheta2Cont(motion_primitives_[tind][mind].end_grid_phi, 244 | phi_grid_resolution_), 245 | common::DiscTheta2Cont(motion_primitives_[tind][mind].start_grid_phi, 246 | phi_grid_resolution_))); 247 | 248 | double angular_time = 249 | angular_distance / 250 | ((M_PI / 4.0) / env_cfg_.timetoturn45degsinplace_secs); 251 | // make the cost the max of the two times 252 | motion_primitives_[tind][mind].cost = static_cast( 253 | std::ceil(1000 * std::max(linear_time, angular_time))); 254 | // use any additional cost multiplier 255 | motion_primitives_[tind][mind].cost *= 256 | motion_primitives_[tind][mind].multiplier; 257 | 258 | // now compute the intersecting cells for this motion (including ignoring 259 | // the source footprint) 260 | polygon_bounds_.Add(common::FootprintHelper::GetMotionXYCells( 261 | env_cfg_.footprint, motion_primitives_[tind][mind].intermptV, 262 | &motion_primitives_[tind][mind].intersectingcellsV, 263 | env_cfg_.xy_grid_resolution)); 264 | } 265 | } 266 | 267 | LOG(INFO) << "bounding box of the polygon: minx=" 268 | << polygon_bounds_.minimum().x() 269 | << ", maxx=" << polygon_bounds_.maximum().x() 270 | << ", miny=" << polygon_bounds_.minimum().y() 271 | << ", maxy=" << polygon_bounds_.maximum().y(); 272 | } 273 | 274 | } // namespace primitive_generator 275 | } // namespace lattice_path_planner 276 | -------------------------------------------------------------------------------- /lattice_path_planner/lattice_path_planner_ros.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2023, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | * Author: Jian Wen (nkuwenjian@gmail.com) 32 | *****************************************************************************/ 33 | 34 | #include "lattice_path_planner/lattice_path_planner_ros.h" 35 | 36 | #include 37 | 38 | #include "costmap_2d/inflation_layer.h" 39 | #include "nav_msgs/Path.h" 40 | #include "pluginlib/class_list_macros.hpp" 41 | #include "tf/tf.h" 42 | #include "tf2/LinearMath/Quaternion.h" 43 | 44 | PLUGINLIB_EXPORT_CLASS(lattice_path_planner::LatticePathPlannerROS, 45 | nav_core::BaseGlobalPlanner) 46 | 47 | namespace lattice_path_planner { 48 | 49 | void LatticePathPlannerROS::initialize(std::string name, 50 | costmap_2d::Costmap2DROS* costmap_ros) { 51 | // Check if LatticePathPlannerROS has been initialized. 52 | if (initialized_) { 53 | LOG(INFO) << "LatticePathPlannerROS has been initialized."; 54 | return; 55 | } 56 | 57 | // Check and update costmap. 58 | if (!UpdateCostmap(costmap_ros)) { 59 | LOG(ERROR) << "Failed to update costmap."; 60 | return; 61 | } 62 | 63 | // Get parameters through ROS parameter server. 64 | ros::NodeHandle private_nh("~/" + name); 65 | VLOG(4) << "Name is " << name; 66 | name_ = name; 67 | double nominalvel_mpersecs = 0.0; 68 | double timetoturn45degsinplace_secs = 0.0; 69 | double vehicle_length = 0.0; 70 | double vehicle_width = 0.0; 71 | if (!LoadRosParamFromNodeHandle(private_nh, &nominalvel_mpersecs, 72 | &timetoturn45degsinplace_secs, 73 | &vehicle_length, &vehicle_width)) { 74 | LOG(ERROR) << "Failed to load ROS parameters."; 75 | return; 76 | } 77 | 78 | // Update robot footprint. 79 | std::vector footprint = 80 | GetFootprint(costmap_ros_->getRobotFootprint()); 81 | 82 | uint8_t cost_possibly_circumscribed_thresh = ComputeCircumscribedCost(); 83 | if (cost_possibly_circumscribed_thresh == 0U) { 84 | // Unfortunately, the inflation_radius is not taken into account by 85 | // inflation_layer->computeCost(). If inflation_radius is smaller than 86 | // the circumscribed radius, SBPL will ignore some obstacles, but we 87 | // cannot detect this problem. If the cost_scaling_factor is too large, 88 | // SBPL won't run into obstacles, but will always perform an expensive 89 | // footprint check, no matter how far the nearest obstacle is. 90 | LOG(ERROR) << std::fixed 91 | << "The costmap value at the robot's circumscribed radius (" 92 | << layered_costmap_->getCircumscribedRadius() << " m) is 0."; 93 | return; 94 | } 95 | 96 | // Create and initialize LatticePathPlannerROS. 97 | planner_ = std::make_unique(); 98 | planner_->Init(static_cast(costmap_2d_->getSizeInCellsX()), 99 | static_cast(costmap_2d_->getSizeInCellsY()), 100 | costmap_2d_->getResolution(), costmap_2d::LETHAL_OBSTACLE, 101 | costmap_2d::INSCRIBED_INFLATED_OBSTACLE, 102 | cost_possibly_circumscribed_thresh, nominalvel_mpersecs, 103 | timetoturn45degsinplace_secs, footprint, 104 | const_cast(primitive_filename_.c_str())); 105 | 106 | visualizer_ = std::make_unique(); 107 | visualizer_->Initialize(name, vehicle_length, vehicle_width); 108 | 109 | initialized_ = true; 110 | LOG(INFO) << "LatticePathPlannerROS is initialized successfully."; 111 | } 112 | 113 | bool LatticePathPlannerROS::UpdateCostmap( 114 | costmap_2d::Costmap2DROS* costmap_ros) { 115 | if (costmap_ros == nullptr) { 116 | LOG(ERROR) << "costmap_ros == nullptr"; 117 | return false; 118 | } 119 | 120 | costmap_ros_ = costmap_ros; 121 | costmap_2d_ = costmap_ros->getCostmap(); 122 | layered_costmap_ = costmap_ros->getLayeredCostmap(); 123 | if (costmap_2d_ == nullptr || layered_costmap_ == nullptr) { 124 | LOG(ERROR) << "costmap_2d_ == nullptr || layered_costmap_ == nullptr"; 125 | return false; 126 | } 127 | return true; 128 | } 129 | 130 | bool LatticePathPlannerROS::LoadRosParamFromNodeHandle( 131 | const ros::NodeHandle& nh, double* nominalvel_mpersecs, 132 | double* timetoturn45degsinplace_secs, double* vehicle_length, 133 | double* vehicle_width) { 134 | // Sanity checks. 135 | CHECK_NOTNULL(nominalvel_mpersecs); 136 | CHECK_NOTNULL(timetoturn45degsinplace_secs); 137 | CHECK_NOTNULL(costmap_2d_); 138 | 139 | nh.param("nominalvel_mpersecs", *nominalvel_mpersecs, 0.4); 140 | nh.param("timetoturn45degsinplace_secs", *timetoturn45degsinplace_secs, 0.6); 141 | nh.param("treat_unknown_as_free", treat_unknown_as_free_, true); 142 | if (!nh.getParam("vehicle_length", *vehicle_length)) { 143 | LOG(ERROR) << "Failed to set vehicle_length"; 144 | return false; 145 | } 146 | if (!nh.getParam("vehicle_width", *vehicle_width)) { 147 | LOG(ERROR) << "Failed to set vehicle_width"; 148 | return false; 149 | } 150 | if (!nh.getParam("primitive_filename", primitive_filename_)) { 151 | LOG(ERROR) << "Failed to set primitive_filename"; 152 | return false; 153 | } 154 | 155 | VLOG(4) << std::fixed << "nominalvel_mpersecs: " << *nominalvel_mpersecs; 156 | VLOG(4) << std::fixed 157 | << "timetoturn45degsinplace_secs: " << *timetoturn45degsinplace_secs; 158 | VLOG(4) << std::boolalpha 159 | << "treat_unknown_as_free: " << treat_unknown_as_free_; 160 | VLOG(4) << std::fixed << "vehicle_length: " << *vehicle_length; 161 | VLOG(4) << std::fixed << "vehicle_width: " << *vehicle_width; 162 | VLOG(4) << "primitive_filename: " << primitive_filename_; 163 | return true; 164 | } 165 | 166 | uint8_t LatticePathPlannerROS::ComputeCircumscribedCost() const { 167 | // Sanity checks. 168 | CHECK_NOTNULL(layered_costmap_); 169 | CHECK_NOTNULL(costmap_2d_); 170 | 171 | std::vector>* plugins = 172 | layered_costmap_->getPlugins(); 173 | if (plugins == nullptr) { 174 | LOG(ERROR) << "plugins == nullptr"; 175 | return 0U; 176 | } 177 | 178 | // Check if the costmap has an inflation layer. 179 | for (auto& plugin : *plugins) { 180 | auto inflation_layer = 181 | boost::dynamic_pointer_cast(plugin); 182 | if (inflation_layer != nullptr) { 183 | return inflation_layer->computeCost( 184 | layered_costmap_->getCircumscribedRadius() / 185 | costmap_2d_->getResolution()); 186 | } 187 | } 188 | return 0U; 189 | } 190 | 191 | std::vector LatticePathPlannerROS::GetFootprint( 192 | const std::vector& robot_footprint) { 193 | std::vector footprint; 194 | footprint.reserve(robot_footprint.size()); 195 | for (const geometry_msgs::Point& point : robot_footprint) { 196 | footprint.emplace_back(point.x, point.y); 197 | } 198 | return footprint; 199 | } 200 | 201 | void LatticePathPlannerROS::GetStartAndEndConfigurations( 202 | const geometry_msgs::PoseStamped& start, 203 | const geometry_msgs::PoseStamped& goal, double origin_x, double origin_y, 204 | double* start_x, double* start_y, double* start_phi, double* end_x, 205 | double* end_y, double* end_phi) { 206 | // Sanity checks. 207 | CHECK_NOTNULL(start_x); 208 | CHECK_NOTNULL(start_y); 209 | CHECK_NOTNULL(start_phi); 210 | CHECK_NOTNULL(end_x); 211 | CHECK_NOTNULL(end_y); 212 | CHECK_NOTNULL(end_phi); 213 | 214 | VLOG(4) << std::fixed << "start point: " << start.pose.position.x << "," 215 | << start.pose.position.y; 216 | VLOG(4) << std::fixed << "end point: " << goal.pose.position.x << "," 217 | << goal.pose.position.y; 218 | 219 | // Start configuration. 220 | *start_x = start.pose.position.x - origin_x; 221 | *start_y = start.pose.position.y - origin_y; 222 | *start_phi = tf::getYaw(start.pose.orientation); 223 | 224 | // End configuration. 225 | *end_x = goal.pose.position.x - origin_x; 226 | *end_y = goal.pose.position.y - origin_y; 227 | *end_phi = tf::getYaw(goal.pose.orientation); 228 | } 229 | 230 | std::vector> LatticePathPlannerROS::GetGridMap( 231 | const uint8_t* char_map, unsigned int size_x, unsigned int size_y, 232 | bool treat_unknown_as_free) { 233 | if (char_map == nullptr) { 234 | LOG(ERROR) << "char_map == nullptr"; 235 | return std::vector>(); 236 | } 237 | std::vector> grid_map; 238 | grid_map.resize(size_x); 239 | for (unsigned int i = 0U; i < size_x; ++i) { 240 | grid_map[i].resize(size_y); 241 | for (unsigned int j = 0U; j < size_y; ++j) { 242 | grid_map[i][j] = char_map[i + j * size_x]; 243 | if (treat_unknown_as_free && 244 | grid_map[i][j] == costmap_2d::NO_INFORMATION) { 245 | grid_map[i][j] = costmap_2d::FREE_SPACE; 246 | } 247 | } 248 | } 249 | return grid_map; 250 | } 251 | 252 | bool LatticePathPlannerROS::makePlan( 253 | const geometry_msgs::PoseStamped& start, 254 | const geometry_msgs::PoseStamped& goal, 255 | std::vector& plan) { 256 | // Check if LatticePathPlannerROS has been initialized. 257 | if (!initialized_) { 258 | LOG(ERROR) << "LatticePathPlannerROS has not been initialized."; 259 | return false; 260 | } 261 | 262 | // Get start and end configurations. 263 | double start_x; 264 | double start_y; 265 | double start_phi; 266 | double end_x; 267 | double end_y; 268 | double end_phi; 269 | GetStartAndEndConfigurations(start, goal, costmap_2d_->getOriginX(), 270 | costmap_2d_->getOriginY(), &start_x, &start_y, 271 | &start_phi, &end_x, &end_y, &end_phi); 272 | 273 | // Get map data from cost map. 274 | std::vector> grid_map = 275 | GetGridMap(costmap_2d_->getCharMap(), costmap_2d_->getSizeInCellsX(), 276 | costmap_2d_->getSizeInCellsY(), treat_unknown_as_free_); 277 | if (grid_map.empty()) { 278 | LOG(ERROR) << "Failed to get grid map."; 279 | return false; 280 | } 281 | 282 | // Run lattice A star planner. 283 | VLOG(4) << "Run lattice A-star planner."; 284 | lattice_a_star::LatticeAStarResult result; 285 | if (!planner_->Plan(start_x, start_y, start_phi, end_x, end_y, end_phi, 286 | std::move(grid_map), &result)) { 287 | return false; 288 | } 289 | 290 | // Populate global plan. 291 | PopulateGlobalPlan(result, start.header, costmap_2d_->getOriginX(), 292 | costmap_2d_->getOriginY(), &plan); 293 | 294 | visualizer_->Visualize(plan); 295 | 296 | return true; 297 | } 298 | 299 | void LatticePathPlannerROS::PopulateGlobalPlan( 300 | const lattice_a_star::LatticeAStarResult& searched_path, 301 | const std_msgs::Header& header, double origin_x, double origin_y, 302 | std::vector* plan) { 303 | // Sanity checks. 304 | CHECK_NOTNULL(plan); 305 | CHECK_EQ(searched_path.x.size(), searched_path.y.size()); 306 | CHECK_EQ(searched_path.x.size(), searched_path.phi.size()); 307 | const std::size_t N = searched_path.x.size(); 308 | 309 | plan->clear(); 310 | geometry_msgs::PoseStamped pose_stamped; 311 | pose_stamped.header = header; 312 | 313 | for (std::size_t i = 0U; i < N; ++i) { 314 | pose_stamped.pose.position.x = searched_path.x[i] + origin_x; 315 | pose_stamped.pose.position.y = searched_path.y[i] + origin_y; 316 | pose_stamped.pose.orientation = 317 | tf::createQuaternionMsgFromYaw(searched_path.phi[i]); 318 | plan->push_back(pose_stamped); 319 | } 320 | } 321 | 322 | } // namespace lattice_path_planner 323 | -------------------------------------------------------------------------------- /lattice_path_planner/grid_search/grid_search.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #include "lattice_path_planner/grid_search/grid_search.h" 33 | 34 | #include 35 | #include // NOLINT 36 | #include 37 | 38 | namespace lattice_path_planner { 39 | namespace grid_search { 40 | 41 | void GridSearch::Init(int max_grid_x, int max_grid_y, double xy_grid_resolution, 42 | uint8_t obsthresh, 43 | TerminationCondition termination_condition) { 44 | if (initialized_) { 45 | LOG(INFO) << "GridSearch has been initialized."; 46 | return; 47 | } 48 | 49 | max_grid_x_ = max_grid_x; 50 | max_grid_y_ = max_grid_y; 51 | xy_grid_resolution_ = xy_grid_resolution; 52 | obsthresh_ = obsthresh; 53 | termination_condition_ = termination_condition; 54 | 55 | open_list_ = std::make_unique(); 56 | 57 | dp_lookup_table_.resize(max_grid_x_); 58 | for (int grid_x = 0; grid_x < max_grid_x; ++grid_x) { 59 | for (int grid_y = 0; grid_y < max_grid_y; ++grid_y) { 60 | dp_lookup_table_[grid_x].emplace_back(grid_x, grid_y); 61 | } 62 | } 63 | 64 | ComputeGridSearchActions(); 65 | initialized_ = true; 66 | LOG(INFO) << "GridSearch is initialized successfully."; 67 | } 68 | 69 | GridSearch::~GridSearch() { open_list_->Clear(); } 70 | 71 | void GridSearch::Clear() { 72 | // clean up heap elements in open list 73 | open_list_->Clear(); 74 | 75 | // clear closed list 76 | closed_list_.clear(); 77 | closed_list_.resize(max_grid_x_ * max_grid_y_, 78 | common::Node::NodeStatus::kOpen); 79 | 80 | start_node_ = nullptr; 81 | end_node_ = nullptr; 82 | } 83 | 84 | bool GridSearch::GenerateGridPath( 85 | int sx, int sy, int ex, int ey, 86 | const std::vector>& grid_map, 87 | GridAStarResult* result) { 88 | if (!initialized_) { 89 | LOG(ERROR) << "GridSearch has not been initialized."; 90 | return false; 91 | } 92 | 93 | const auto start_timestamp = std::chrono::system_clock::now(); 94 | 95 | // clean up previous planning result 96 | Clear(); 97 | grid_map_ = grid_map; 98 | iterations_++; 99 | 100 | // check the validity of start/goal 101 | if (!SetStartAndEndConfiguration(sx, sy, ex, ey)) { 102 | return false; 103 | } 104 | 105 | // initialize start node and insert it into heap 106 | start_node_->set_g(0); 107 | start_node_->set_h(CalcHeuCost(sx, sy)); 108 | open_list_->Insert(start_node_, GetKey(start_node_)); 109 | 110 | float term_factor = GetTerminationFactor(termination_condition_); 111 | 112 | // grid search begins 113 | std::size_t explored_node_num = 0U; 114 | while (!open_list_->Empty() && 115 | end_node_->g() > 116 | static_cast(term_factor * 117 | static_cast(open_list_->GetMinKey()))) { 118 | auto* node = dynamic_cast(open_list_->Pop()); 119 | CHECK_NOTNULL(node); 120 | CHECK_NE(node->g(), common::kInfiniteCost); 121 | closed_list_[CalcGridXYIndex(node->grid_x(), node->grid_y())] = 122 | common::Node::NodeStatus::kClosed; 123 | 124 | // new expand 125 | ++explored_node_num; 126 | UpdateSuccs(node); 127 | } 128 | 129 | const auto end_timestamp = std::chrono::system_clock::now(); 130 | const std::chrono::duration diff = end_timestamp - start_timestamp; 131 | VLOG(4) << "time used is " << diff.count() * 1e3 << " ms"; 132 | VLOG(4) << "explored node num is " << explored_node_num; 133 | VLOG(4) << "2Dsolcost_inms = " << end_node_->g(); 134 | VLOG(4) << "largestoptfval = " << open_list_->GetMinKey(); 135 | VLOG(4) << "heap size = " << open_list_->Size(); 136 | 137 | if (end_node_->g() == common::kInfiniteCost) { 138 | LOG(ERROR) << "Grid searching return infinite cost (open_list ran out)"; 139 | return false; 140 | } 141 | if (termination_condition_ == TerminationCondition::kOptPath) { 142 | LoadGridAStarResult(result); 143 | } 144 | return true; 145 | } 146 | 147 | bool GridSearch::IsWithinMap(const int grid_x, const int grid_y) const { 148 | return grid_x >= 0 && grid_x < max_grid_x_ && grid_y >= 0 && 149 | grid_y < max_grid_y_; 150 | } 151 | 152 | bool GridSearch::IsValidCell(const int grid_x, const int grid_y) const { 153 | if (!IsWithinMap(grid_x, grid_y)) { 154 | return false; 155 | } 156 | if (grid_map_[grid_x][grid_y] >= obsthresh_) { 157 | return false; 158 | } 159 | return true; 160 | } 161 | 162 | int GridSearch::CalcGridXYIndex(const int grid_x, const int grid_y) const { 163 | DCHECK(IsWithinMap(grid_x, grid_y)); 164 | return grid_x + grid_y * max_grid_x_; 165 | } 166 | 167 | int GridSearch::GetKey(const Node2d* node) const { 168 | CHECK_NOTNULL(node); 169 | return termination_condition_ == TerminationCondition::kOptPath 170 | ? node->g() + node->h() 171 | : node->g(); 172 | } 173 | 174 | int GridSearch::GetActionCost(int curr_x, int curr_y, int action_id) const { 175 | CHECK(IsValidCell(curr_x, curr_y)); 176 | const int succ_x = curr_x + actions_.dx[action_id]; 177 | const int succ_y = curr_y + actions_.dy[action_id]; 178 | CHECK(IsValidCell(succ_x, succ_y)); 179 | 180 | uint8_t cost = std::max(grid_map_[curr_x][curr_y], grid_map_[succ_x][succ_y]); 181 | if (common::kNumOfGridSearchActions > 8) { 182 | if (action_id > 7) { 183 | int x = curr_x + actions_.dx0intersects[action_id]; 184 | int y = curr_y + actions_.dy0intersects[action_id]; 185 | CHECK(IsWithinMap(x, y)); 186 | cost = std::max(cost, grid_map_[x][y]); 187 | x = curr_x + actions_.dx1intersects[action_id]; 188 | y = curr_y + actions_.dy1intersects[action_id]; 189 | CHECK(IsWithinMap(x, y)); 190 | cost = std::max(cost, grid_map_[x][y]); 191 | } 192 | } 193 | if (cost >= obsthresh_) { 194 | return common::kInfiniteCost; 195 | } 196 | return static_cast(cost + 1) * actions_.dxy_distance_mm[action_id]; 197 | } 198 | 199 | void GridSearch::ComputeGridSearchActions() { 200 | // initialize some constants for 2D search 201 | // up 202 | actions_.dx[0] = 0; 203 | actions_.dy[0] = 1; 204 | actions_.dx0intersects[0] = -1; 205 | actions_.dy0intersects[0] = -1; 206 | // up right 207 | actions_.dx[1] = 1; 208 | actions_.dy[1] = 1; 209 | actions_.dx0intersects[1] = -1; 210 | actions_.dy0intersects[1] = -1; 211 | // right 212 | actions_.dx[2] = 1; 213 | actions_.dy[2] = 0; 214 | actions_.dx0intersects[2] = -1; 215 | actions_.dy0intersects[2] = -1; 216 | // down right 217 | actions_.dx[3] = 1; 218 | actions_.dy[3] = -1; 219 | actions_.dx0intersects[3] = -1; 220 | actions_.dy0intersects[3] = -1; 221 | // down 222 | actions_.dx[4] = 0; 223 | actions_.dy[4] = -1; 224 | actions_.dx0intersects[4] = -1; 225 | actions_.dy0intersects[4] = -1; 226 | // down left 227 | actions_.dx[5] = -1; 228 | actions_.dy[5] = -1; 229 | actions_.dx0intersects[5] = -1; 230 | actions_.dy0intersects[5] = -1; 231 | // left 232 | actions_.dx[6] = -1; 233 | actions_.dy[6] = 0; 234 | actions_.dx0intersects[6] = -1; 235 | actions_.dy0intersects[6] = -1; 236 | // up left 237 | actions_.dx[7] = -1; 238 | actions_.dy[7] = 1; 239 | actions_.dx0intersects[7] = -1; 240 | actions_.dy0intersects[7] = -1; 241 | 242 | // Note: these actions have to be starting at 8 and through 15, since they 243 | // get multiplied correspondingly in Dijkstra's search based on index 244 | if (common::kNumOfGridSearchActions == 16) { 245 | actions_.dx[8] = 1; 246 | actions_.dy[8] = 2; 247 | actions_.dx0intersects[8] = 0; 248 | actions_.dy0intersects[8] = 1; 249 | actions_.dx1intersects[8] = 1; 250 | actions_.dy1intersects[8] = 1; 251 | actions_.dx[9] = 2; 252 | actions_.dy[9] = 1; 253 | actions_.dx0intersects[9] = 1; 254 | actions_.dy0intersects[9] = 0; 255 | actions_.dx1intersects[9] = 1; 256 | actions_.dy1intersects[9] = 1; 257 | actions_.dx[10] = 2; 258 | actions_.dy[10] = -1; 259 | actions_.dx0intersects[10] = 1; 260 | actions_.dy0intersects[10] = 0; 261 | actions_.dx1intersects[10] = 1; 262 | actions_.dy1intersects[10] = -1; 263 | actions_.dx[11] = 1; 264 | actions_.dy[11] = -2; 265 | actions_.dx0intersects[11] = 0; 266 | actions_.dy0intersects[11] = -1; 267 | actions_.dx1intersects[11] = 1; 268 | actions_.dy1intersects[11] = -1; 269 | actions_.dx[12] = -1; 270 | actions_.dy[12] = -2; 271 | actions_.dx0intersects[12] = 0; 272 | actions_.dy0intersects[12] = -1; 273 | actions_.dx1intersects[12] = -1; 274 | actions_.dy1intersects[12] = -1; 275 | actions_.dx[13] = -2; 276 | actions_.dy[13] = -1; 277 | actions_.dx0intersects[13] = -1; 278 | actions_.dy0intersects[13] = 0; 279 | actions_.dx1intersects[13] = -1; 280 | actions_.dy1intersects[13] = -1; 281 | actions_.dx[14] = -2; 282 | actions_.dy[14] = 1; 283 | actions_.dx0intersects[14] = -1; 284 | actions_.dy0intersects[14] = 0; 285 | actions_.dx1intersects[14] = -1; 286 | actions_.dy1intersects[14] = 1; 287 | actions_.dx[15] = -1; 288 | actions_.dy[15] = 2; 289 | actions_.dx0intersects[15] = 0; 290 | actions_.dy0intersects[15] = 1; 291 | actions_.dx1intersects[15] = -1; 292 | actions_.dy1intersects[15] = 1; 293 | } 294 | 295 | // compute distances 296 | for (int dind = 0; dind < common::kNumOfGridSearchActions; dind++) { 297 | if (actions_.dx[dind] != 0 && actions_.dy[dind] != 0) { 298 | if (dind <= 7) { 299 | // the cost of a diagonal move in millimeters 300 | actions_.dxy_distance_mm[dind] = 301 | static_cast(xy_grid_resolution_ * 1414); 302 | } else { 303 | // the cost of a move to 1,2 or 2,1 or so on in millimeters 304 | actions_.dxy_distance_mm[dind] = 305 | static_cast(xy_grid_resolution_ * 2236); 306 | } 307 | } else { 308 | // the cost of a horizontal move in millimeters 309 | actions_.dxy_distance_mm[dind] = 310 | static_cast(xy_grid_resolution_ * 1000); 311 | } 312 | } 313 | } 314 | 315 | bool GridSearch::SetStart(const int start_x, const int start_y) { 316 | if (!IsValidCell(start_x, start_y)) { 317 | return false; 318 | } 319 | start_node_ = GetNode(start_x, start_y); 320 | return true; 321 | } 322 | 323 | bool GridSearch::SetEnd(const int end_x, const int end_y) { 324 | if (!IsValidCell(end_x, end_y)) { 325 | return false; 326 | } 327 | end_node_ = GetNode(end_x, end_y); 328 | return true; 329 | } 330 | 331 | bool GridSearch::SetStartAndEndConfiguration(int sx, int sy, int ex, int ey) { 332 | if (!SetStart(sx, sy)) { 333 | LOG(ERROR) << "VoronoiPlanner is called on invalid start (" << sx << "," 334 | << sy << ")"; 335 | return false; 336 | } 337 | CHECK_NOTNULL(start_node_); 338 | // since the goal has not been set yet, the start node's h value is set to 0 339 | CHECK_EQ(start_node_->h(), 0); 340 | 341 | if (!SetEnd(ex, ey)) { 342 | LOG(ERROR) << "VoronoiPlanner is called on invalid end (" << ex << "," << ey 343 | << ")"; 344 | return false; 345 | } 346 | CHECK_NOTNULL(end_node_); 347 | CHECK_EQ(end_node_->h(), 0); 348 | return true; 349 | } 350 | 351 | Node2d* GridSearch::GetNode(const int grid_x, const int grid_y) { 352 | DCHECK(IsWithinMap(grid_x, grid_y)); 353 | Node2d* node = &dp_lookup_table_[grid_x][grid_y]; 354 | if (node->iterations() != iterations_) { 355 | node->set_h(CalcHeuCost(grid_x, grid_y)); 356 | node->set_g(common::kInfiniteCost); 357 | node->set_pre_node(nullptr); 358 | node->set_heap_index(0); 359 | node->set_iterations(iterations_); 360 | } 361 | return node; 362 | } 363 | 364 | void GridSearch::UpdateSuccs(const Node2d* curr_node) { 365 | CHECK_NOTNULL(curr_node); 366 | const int curr_x = curr_node->grid_x(); 367 | const int curr_y = curr_node->grid_y(); 368 | for (int action_id = 0; action_id < common::kNumOfGridSearchActions; 369 | ++action_id) { 370 | const int succ_x = curr_x + actions_.dx[action_id]; 371 | const int succ_y = curr_y + actions_.dy[action_id]; 372 | if (!IsValidCell(succ_x, succ_y)) { 373 | continue; 374 | } 375 | if (closed_list_[CalcGridXYIndex(succ_x, succ_y)] == 376 | common::Node::NodeStatus::kClosed) { 377 | continue; 378 | } 379 | // get action cost 380 | int action_cost = GetActionCost(curr_x, curr_y, action_id); 381 | if (action_cost == common::kInfiniteCost) { 382 | continue; 383 | } 384 | 385 | Node2d* succ_node = GetNode(succ_x, succ_y); 386 | // see if we can decrease the value of successive node taking into account 387 | // the cost of action 388 | if (succ_node->g() > curr_node->g() + action_cost) { 389 | succ_node->set_g(curr_node->g() + action_cost); 390 | succ_node->set_pre_node(curr_node); 391 | 392 | // re-insert into heap if not closed yet 393 | if (succ_node->heap_index() == 0) { 394 | open_list_->Insert(succ_node, GetKey(succ_node)); 395 | } else { 396 | open_list_->Update(succ_node, GetKey(succ_node)); 397 | } 398 | } 399 | } 400 | } 401 | 402 | int GridSearch::CalcHeuCost(const int grid_x, const int grid_y) const { 403 | if (end_node_ == nullptr) { 404 | return 0; 405 | } 406 | return static_cast(1000 * xy_grid_resolution_ * 407 | std::max(std::abs(grid_x - end_node_->grid_x()), 408 | std::abs(grid_y - end_node_->grid_y()))); 409 | } 410 | 411 | void GridSearch::LoadGridAStarResult(GridAStarResult* result) const { 412 | if (result == nullptr) { 413 | return; 414 | } 415 | result->path_cost = end_node_->g(); 416 | const Node2d* node = end_node_; 417 | std::vector grid_a_x; 418 | std::vector grid_a_y; 419 | while (node != nullptr) { 420 | grid_a_x.push_back(node->grid_x()); 421 | grid_a_y.push_back(node->grid_y()); 422 | node = node->pre_node(); 423 | } 424 | std::reverse(grid_a_x.begin(), grid_a_x.end()); 425 | std::reverse(grid_a_y.begin(), grid_a_y.end()); 426 | result->x = std::move(grid_a_x); 427 | result->y = std::move(grid_a_y); 428 | } 429 | 430 | int GridSearch::CheckDpMap(const int grid_x, const int grid_y) { 431 | if (!initialized_) { 432 | LOG(ERROR) << "GridSearch has not been initialized."; 433 | return common::kInfiniteCost; 434 | } 435 | 436 | const Node2d* node = GetNode(grid_x, grid_y); 437 | CHECK_NOTNULL(node); 438 | CHECK_EQ(node->iterations(), iterations_); 439 | return node->g(); 440 | } 441 | 442 | float GridSearch::GetTerminationFactor( 443 | TerminationCondition termination_condition) { 444 | float term_factor = 0.0F; 445 | switch (termination_condition) { 446 | case TerminationCondition::kOptPath: 447 | term_factor = 1.0F; 448 | break; 449 | case TerminationCondition::kTwentyPercentOverOptPath: 450 | term_factor = 1.0F / 1.2F; 451 | break; 452 | case TerminationCondition::kTwoTimesOptPath: 453 | term_factor = 0.5F; 454 | break; 455 | case TerminationCondition::kThreeTimesOptPath: 456 | term_factor = 1.0F / 3.0F; 457 | break; 458 | case TerminationCondition::kAllCells: 459 | term_factor = 0.0F; 460 | break; 461 | default: 462 | term_factor = 0.0F; 463 | } 464 | return term_factor; 465 | } 466 | 467 | } // namespace grid_search 468 | } // namespace lattice_path_planner 469 | -------------------------------------------------------------------------------- /lattice_path_planner/lattice_a_star/lattice_a_star.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #include "lattice_path_planner/lattice_a_star/lattice_a_star.h" 33 | 34 | #include // NOLINT 35 | 36 | #include "glog/logging.h" 37 | 38 | #include "lattice_path_planner/common/footprint_helper.h" 39 | #include "lattice_path_planner/common/utils.h" 40 | 41 | namespace lattice_path_planner { 42 | namespace lattice_a_star { 43 | 44 | LatticeAStar::~LatticeAStar() { open_list_->Clear(); } 45 | 46 | void LatticeAStar::Init(int max_grid_x, int max_grid_y, 47 | double xy_grid_resolution, uint8_t obsthresh, 48 | uint8_t cost_inscribed_thresh, 49 | int cost_possibly_circumscribed_thresh, 50 | double nominalvel_mpersecs, 51 | double timetoturn45degsinplace_secs, 52 | const std::vector& footprint, 53 | char* motPrimFilename) { 54 | if (initialized_) { 55 | LOG(INFO) << "LatticeAStar has been initialized."; 56 | return; 57 | } 58 | 59 | env_cfg_.max_grid_x = max_grid_x; 60 | env_cfg_.max_grid_y = max_grid_y; 61 | env_cfg_.xy_grid_resolution = xy_grid_resolution; 62 | env_cfg_.obsthresh = obsthresh; 63 | env_cfg_.cost_inscribed_thresh = cost_inscribed_thresh; 64 | env_cfg_.cost_possibly_circumscribed_thresh = 65 | cost_possibly_circumscribed_thresh; 66 | env_cfg_.nominalvel_mpersecs = nominalvel_mpersecs; 67 | env_cfg_.timetoturn45degsinplace_secs = timetoturn45degsinplace_secs; 68 | env_cfg_.footprint = footprint; 69 | 70 | motion_primitive_generator_ = 71 | std::make_unique(env_cfg_); 72 | motion_primitive_generator_->Init(motPrimFilename); 73 | 74 | grid_a_star_heuristic_generator_ = 75 | std::make_unique(); 76 | grid_a_star_heuristic_generator_->Init( 77 | env_cfg_.max_grid_x, env_cfg_.max_grid_y, env_cfg_.xy_grid_resolution, 78 | env_cfg_.cost_inscribed_thresh, 79 | grid_search::GridSearch::TerminationCondition::kTwoTimesOptPath); 80 | open_list_ = std::make_unique(); 81 | 82 | lattice_lookup_table_.resize(env_cfg_.phi_grid_resolution); 83 | for (int i = 0; i < env_cfg_.phi_grid_resolution; ++i) { 84 | lattice_lookup_table_[i].resize(env_cfg_.max_grid_x * env_cfg_.max_grid_y); 85 | } 86 | initialized_ = true; 87 | LOG(INFO) << "LatticeAStar is initialized successfully."; 88 | } 89 | 90 | bool LatticeAStar::SetStart(double start_x, double start_y, double start_phi) { 91 | int start_grid_x; 92 | int start_grid_y; 93 | int start_grid_phi; 94 | WorldToGrid(start_x, start_y, start_phi, &start_grid_x, &start_grid_y, 95 | &start_grid_phi); 96 | 97 | // create start node 98 | start_node_ = GetNode(start_grid_x, start_grid_y, start_grid_phi); 99 | if (!ValidityCheck(start_node_)) { 100 | LOG(ERROR) << "Invalid start node: (" << start_grid_x << "," << start_grid_y 101 | << "," << start_grid_phi << ")"; 102 | return false; 103 | } 104 | return true; 105 | } 106 | 107 | bool LatticeAStar::SetEnd(double end_x, double end_y, double end_phi) { 108 | int end_grid_x; 109 | int end_grid_y; 110 | int end_grid_phi; 111 | WorldToGrid(end_x, end_y, end_phi, &end_grid_x, &end_grid_y, &end_grid_phi); 112 | 113 | // create end node. 114 | end_node_ = GetNode(end_grid_x, end_grid_y, end_grid_phi); 115 | if (!ValidityCheck(end_node_)) { 116 | LOG(ERROR) << "Invalid end node: (" << end_grid_x << "," << end_grid_y 117 | << "," << end_grid_phi << ")"; 118 | return false; 119 | } 120 | return true; 121 | } 122 | 123 | void LatticeAStar::Clear() { 124 | // clean up heap elements in open list 125 | open_list_->Clear(); 126 | 127 | // clear closed list 128 | closed_list_.clear(); 129 | closed_list_.resize(env_cfg_.phi_grid_resolution); 130 | for (int i = 0; i < env_cfg_.phi_grid_resolution; ++i) { 131 | closed_list_[i].resize(env_cfg_.max_grid_x * env_cfg_.max_grid_y, 132 | common::Node::NodeStatus::kOpen); 133 | } 134 | 135 | start_node_ = nullptr; 136 | end_node_ = nullptr; 137 | } 138 | 139 | bool LatticeAStar::Plan(double start_x, double start_y, double start_phi, 140 | double end_x, double end_y, double end_phi, 141 | std::vector>&& grid_map, 142 | LatticeAStarResult* result) { 143 | if (!initialized_) { 144 | LOG(ERROR) << "LatticeAStar has not been initialized."; 145 | return false; 146 | } 147 | 148 | const auto start_timestamp = std::chrono::system_clock::now(); 149 | 150 | // clean up previous planning result 151 | Clear(); 152 | iterations_++; 153 | created_node_num_ = 0; 154 | env_cfg_.grid_map = std::move(grid_map); 155 | 156 | if (!SetStart(start_x, start_y, start_phi)) { 157 | return false; 158 | } 159 | CHECK_NOTNULL(start_node_); 160 | // since the goal has not been set yet, the start node's h value is set to 0 161 | CHECK_EQ(start_node_->h(), 0); 162 | 163 | if (!SetEnd(end_x, end_y, end_phi)) { 164 | return false; 165 | } 166 | CHECK_NOTNULL(end_node_); 167 | CHECK_EQ(end_node_->h(), 0); 168 | 169 | // update heuristic function 170 | grid_a_star_heuristic_generator_->GenerateGridPath( 171 | end_node_->grid_x(), end_node_->grid_y(), start_node_->grid_x(), 172 | start_node_->grid_y(), env_cfg_.grid_map, nullptr); 173 | 174 | // initialize start node and insert it into heap 175 | start_node_->set_g(0); 176 | start_node_->set_h(CalcHeuCost(start_node_->grid_x(), start_node_->grid_y())); 177 | open_list_->Insert(start_node_, start_node_->g() + start_node_->h()); 178 | 179 | // Lattice A* begins 180 | std::size_t explored_node_num = 0U; 181 | while (!open_list_->Empty() && end_node_->g() > open_list_->GetMinKey()) { 182 | // get the state 183 | auto* node = dynamic_cast(open_list_->Pop()); 184 | CHECK_NOTNULL(node); 185 | CHECK_NE(node->g(), common::kInfiniteCost); 186 | closed_list_[node->grid_phi()][CalcGridXYIndex( 187 | node->grid_x(), node->grid_y())] = common::Node::NodeStatus::kClosed; 188 | // new expand 189 | ++explored_node_num; 190 | UpdateSuccs(node); 191 | 192 | if (explored_node_num % 100000 == 0 && explored_node_num > 0) { 193 | LOG(INFO) << "expands so far=" << explored_node_num; 194 | } 195 | } 196 | 197 | const auto end_timestamp = std::chrono::system_clock::now(); 198 | const std::chrono::duration time_diff = 199 | end_timestamp - start_timestamp; 200 | LOG(INFO) << "total expands this call=" << explored_node_num 201 | << ", planning time=" << time_diff.count() * 1e3 202 | << " msecs, solution cost=" << end_node_->g() 203 | << ", heap size=" << open_list_->Size() 204 | << ", num of newly created nodes=" << created_node_num_; 205 | 206 | if (end_node_->g() == common::kInfiniteCost) { 207 | LOG(INFO) << "Lattice A* searching failed."; 208 | return false; 209 | } 210 | 211 | LoadLatticeAStarResult(result); 212 | return true; 213 | } 214 | 215 | int LatticeAStar::CalcGridXYIndex(const int grid_x, const int grid_y) const { 216 | DCHECK(IsWithinMap(grid_x, grid_y)); 217 | return grid_x + grid_y * env_cfg_.max_grid_x; 218 | } 219 | 220 | int LatticeAStar::CalcHeuCost(const int grid_x, const int grid_y) const { 221 | if (end_node_ == nullptr) { 222 | return 0; 223 | } 224 | 225 | // computes distances from start state that is grid2D, so it is 226 | // EndX_c EndY_c 227 | int h2D = grid_a_star_heuristic_generator_->CheckDpMap(grid_x, grid_y); 228 | int hEuclid = 229 | static_cast(1000 * EuclidHeuCost(grid_x, grid_y, end_node_->grid_x(), 230 | end_node_->grid_y())); 231 | 232 | // define this function if it is used in the planner (heuristic backward 233 | // search would use it) 234 | return static_cast(static_cast(std::max(h2D, hEuclid)) / 235 | env_cfg_.nominalvel_mpersecs); 236 | } 237 | 238 | double LatticeAStar::EuclidHeuCost(const int x1, const int y1, const int x2, 239 | const int y2) const { 240 | int sqdist = ((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)); 241 | return env_cfg_.xy_grid_resolution * std::sqrt(static_cast(sqdist)); 242 | } 243 | 244 | bool LatticeAStar::WorldToGrid(const double x, const double y, const double phi, 245 | int* grid_x, int* grid_y, int* grid_phi) const { 246 | *grid_x = common::ContXY2Disc(x, env_cfg_.xy_grid_resolution); 247 | *grid_y = common::ContXY2Disc(y, env_cfg_.xy_grid_resolution); 248 | *grid_phi = common::ContTheta2Disc(phi, env_cfg_.phi_grid_resolution); 249 | return true; 250 | } 251 | 252 | bool LatticeAStar::GridToWorld(const int grid_x, const int grid_y, 253 | const int grid_phi, double* x, double* y, 254 | double* phi) const { 255 | if (!IsWithinMap(grid_x, grid_y)) { 256 | return false; 257 | } 258 | if (grid_phi < 0 || grid_phi >= env_cfg_.phi_grid_resolution) { 259 | return false; 260 | } 261 | 262 | *x = common::DiscXY2Cont(grid_x, env_cfg_.xy_grid_resolution); 263 | *y = common::DiscXY2Cont(grid_y, env_cfg_.xy_grid_resolution); 264 | *phi = common::DiscTheta2Cont(grid_phi, env_cfg_.phi_grid_resolution); 265 | return true; 266 | } 267 | 268 | bool LatticeAStar::IsWithinMap(const int grid_x, const int grid_y) const { 269 | return grid_x >= 0 && grid_x < env_cfg_.max_grid_x && grid_y >= 0 && 270 | grid_y < env_cfg_.max_grid_y; 271 | } 272 | 273 | bool LatticeAStar::ValidityCheck(const Node3d* node) const { 274 | CHECK_NOTNULL(node); 275 | int grid_x = node->grid_x(); 276 | int grid_y = node->grid_y(); 277 | int grid_phi = node->grid_phi(); 278 | if (!IsValidCell(grid_x, grid_y)) { 279 | return false; 280 | } 281 | 282 | // compute continuous pose 283 | common::XYThetaPoint pose; 284 | CHECK(GridToWorld(grid_x, grid_y, grid_phi, pose.mutable_x(), 285 | pose.mutable_y(), pose.mutable_theta())); 286 | 287 | // compute covered cells 288 | std::vector covered_cells; 289 | common::FootprintHelper::GetFootprintXYCells( 290 | env_cfg_.footprint, &covered_cells, pose, env_cfg_.xy_grid_resolution); 291 | 292 | // iterate over all covered cells 293 | for (const common::XYCell& cell : covered_cells) { 294 | int x = cell.x(); 295 | int y = cell.y(); 296 | if (!IsWithinMap(x, y) || env_cfg_.grid_map[x][y] >= env_cfg_.obsthresh) { 297 | return false; 298 | } 299 | } 300 | return true; 301 | } 302 | 303 | void LatticeAStar::UpdateSuccs(const Node3d* curr_node) { 304 | CHECK_NOTNULL(curr_node); 305 | const int curr_x = curr_node->grid_x(); 306 | const int curr_y = curr_node->grid_y(); 307 | const int curr_phi = curr_node->grid_phi(); 308 | const std::vector& actions = 309 | motion_primitive_generator_->motion_primitives()[curr_phi]; 310 | 311 | // iterate through every primitive 312 | for (const primitive_generator::Primitive& action : actions) { 313 | const int succ_x = curr_x + action.end_grid_x; 314 | const int succ_y = curr_y + action.end_grid_y; 315 | const int succ_phi = action.end_grid_phi; 316 | if (!IsValidCell(succ_x, succ_y)) { 317 | continue; 318 | } 319 | if (closed_list_[succ_phi][CalcGridXYIndex(succ_x, succ_y)] == 320 | common::Node::NodeStatus::kClosed) { 321 | continue; 322 | } 323 | // get action cost 324 | int action_cost = GetActionCost(curr_x, curr_y, action); 325 | if (action_cost == common::kInfiniteCost) { 326 | continue; 327 | } 328 | 329 | Node3d* succ_node = GetNode(succ_x, succ_y, succ_phi); 330 | // see if we can decrease the value of successive node taking into account 331 | // the cost of action 332 | if (succ_node->g() > curr_node->g() + action_cost) { 333 | succ_node->set_g(curr_node->g() + action_cost); 334 | succ_node->set_pre_node(curr_node); 335 | succ_node->set_action_idx({curr_phi, action.id}); 336 | 337 | // re-insert into heap if not closed yet 338 | if (succ_node->heap_index() == 0) { 339 | open_list_->Insert(succ_node, succ_node->g() + succ_node->h()); 340 | } else { 341 | open_list_->Update(succ_node, succ_node->g() + succ_node->h()); 342 | } 343 | } 344 | } 345 | } 346 | 347 | bool LatticeAStar::IsValidCell(const int grid_x, const int grid_y) const { 348 | if (!IsWithinMap(grid_x, grid_y)) { 349 | return false; 350 | } 351 | if (env_cfg_.grid_map[grid_x][grid_y] >= env_cfg_.cost_inscribed_thresh) { 352 | return false; 353 | } 354 | return true; 355 | } 356 | 357 | int LatticeAStar::GetActionCost( 358 | const int curr_x, const int curr_y, 359 | const primitive_generator::Primitive& action) const { 360 | // TODO(all) - go over bounding box (minpt and maxpt) to test validity and 361 | // skip testing boundaries below, also order intersect cells so that the four 362 | // farthest pts go first 363 | CHECK(IsValidCell(curr_x, curr_y)); 364 | const int succ_x = curr_x + action.end_grid_x; 365 | const int succ_y = curr_y + action.end_grid_y; 366 | CHECK(IsValidCell(succ_x, succ_y)); 367 | 368 | // need to iterate over discretized center cells and compute cost based on 369 | // them 370 | uint8_t maxcellcost = 0; 371 | for (const common::XYCell& cell : action.interm2DcellsV) { 372 | const int x = curr_x + cell.x(); 373 | const int y = curr_y + cell.y(); 374 | if (!IsValidCell(x, y)) { 375 | return common::kInfiniteCost; 376 | } 377 | maxcellcost = std::max(maxcellcost, env_cfg_.grid_map[x][y]); 378 | } 379 | 380 | // check collisions that for the particular footprint orientation along the 381 | // action 382 | if (env_cfg_.footprint.size() > 1 && 383 | static_cast(maxcellcost) >= 384 | env_cfg_.cost_possibly_circumscribed_thresh) { 385 | for (const common::XYCell& cell : action.intersectingcellsV) { 386 | // get the cell in the map 387 | int x = curr_x + cell.x(); 388 | int y = curr_y + cell.y(); 389 | 390 | // check validity 391 | if (!IsWithinMap(x, y) || env_cfg_.grid_map[x][y] >= env_cfg_.obsthresh) { 392 | return common::kInfiniteCost; 393 | } 394 | } 395 | } 396 | 397 | // to ensure consistency of h2D: 398 | int currentmaxcost = 399 | static_cast(std::max({maxcellcost, env_cfg_.grid_map[curr_x][curr_y], 400 | env_cfg_.grid_map[succ_x][succ_y]})); 401 | 402 | // use cell cost as multiplicative factor 403 | return static_cast(action.cost) * (currentmaxcost + 1); 404 | // return action.cost; 405 | } 406 | 407 | Node3d* LatticeAStar::GetNode(const int grid_x, const int grid_y, 408 | const int grid_phi) { 409 | int xy_index = CalcGridXYIndex(grid_x, grid_y); 410 | 411 | const Node3dPtr& node = lattice_lookup_table_[grid_phi][xy_index]; 412 | if (node == nullptr) { 413 | lattice_lookup_table_[grid_phi][xy_index] = 414 | std::make_unique(grid_x, grid_y, grid_phi); 415 | node->set_h(CalcHeuCost(grid_x, grid_y)); 416 | node->set_iterations(iterations_); 417 | created_node_num_++; 418 | } 419 | // reset node created in previous planning cycle 420 | if (node->iterations() != iterations_) { 421 | node->set_h(CalcHeuCost(grid_x, grid_y)); 422 | node->set_g(common::kInfiniteCost); 423 | node->set_action_idx({0, 0}); 424 | node->set_pre_node(nullptr); 425 | node->set_heap_index(0); 426 | node->set_iterations(iterations_); 427 | } 428 | return node.get(); 429 | } 430 | 431 | void LatticeAStar::LoadLatticeAStarResult(LatticeAStarResult* result) { 432 | if (result == nullptr) { 433 | return; 434 | } 435 | result->x.clear(); 436 | result->y.clear(); 437 | result->phi.clear(); 438 | 439 | const Node3d* node = end_node_; 440 | std::vector astar_result; 441 | while (node != nullptr) { 442 | astar_result.push_back(node); 443 | node = node->pre_node(); 444 | } 445 | std::reverse(astar_result.begin(), astar_result.end()); 446 | if (astar_result.size() < 2) { 447 | LOG(ERROR) << "A-star result contains less than 2 nodes"; 448 | return; 449 | } 450 | 451 | const std::vector>& 452 | motion_primitives = motion_primitive_generator_->motion_primitives(); 453 | for (std::size_t i = 0U; i < astar_result.size() - 1U; ++i) { 454 | double curr_x = 0.0; 455 | double curr_y = 0.0; 456 | double curr_phi = 0.0; 457 | CHECK(GridToWorld(astar_result[i]->grid_x(), astar_result[i]->grid_y(), 458 | astar_result[i]->grid_phi(), &curr_x, &curr_y, 459 | &curr_phi)); 460 | result->x.push_back(curr_x); 461 | result->y.push_back(curr_y); 462 | result->phi.push_back(curr_phi); 463 | double succ_x = 0.0; 464 | double succ_y = 0.0; 465 | double succ_phi = 0.0; 466 | CHECK(GridToWorld( 467 | astar_result[i + 1]->grid_x(), astar_result[i + 1]->grid_y(), 468 | astar_result[i + 1]->grid_phi(), &succ_x, &succ_y, &succ_phi)); 469 | 470 | const primitive_generator::Primitive& action = 471 | motion_primitives[astar_result[i + 1]->action_idx().first] 472 | [astar_result[i + 1]->action_idx().second]; 473 | CHECK_EQ(astar_result[i]->grid_phi(), action.start_grid_phi); 474 | CHECK_EQ(astar_result[i + 1]->grid_phi(), action.end_grid_phi); 475 | for (std::size_t i = 0U; i < action.intermptV.size(); ++i) { 476 | const double intermpt_x = curr_x + action.intermptV[i].x(); 477 | const double intermpt_y = curr_y + action.intermptV[i].y(); 478 | const double intermpt_phi = action.intermptV[i].theta(); 479 | if (i > 0 && i < action.intermptV.size() - 1) { 480 | result->x.push_back(intermpt_x); 481 | result->y.push_back(intermpt_y); 482 | result->phi.push_back(intermpt_phi); 483 | } 484 | } 485 | } 486 | 487 | common::XYThetaPoint last_point; 488 | CHECK(GridToWorld(astar_result.back()->grid_x(), 489 | astar_result.back()->grid_y(), 490 | astar_result.back()->grid_phi(), last_point.mutable_x(), 491 | last_point.mutable_y(), last_point.mutable_theta())); 492 | result->x.push_back(last_point.x()); 493 | result->y.push_back(last_point.y()); 494 | result->phi.push_back(last_point.theta()); 495 | } 496 | 497 | } // namespace lattice_a_star 498 | } // namespace lattice_path_planner 499 | -------------------------------------------------------------------------------- /third_party/spline/spline.h: -------------------------------------------------------------------------------- 1 | /* 2 | * spline.h 3 | * 4 | * simple cubic spline interpolation library without external 5 | * dependencies 6 | * 7 | * --------------------------------------------------------------------- 8 | * Copyright (C) 2011, 2014, 2016, 2021 Tino Kluge (ttk448 at gmail.com) 9 | * 10 | * This program is free software; you can redistribute it and/or 11 | * modify it under the terms of the GNU General Public License 12 | * as published by the Free Software Foundation; either version 2 13 | * of the License, or (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | * --------------------------------------------------------------------- 23 | * 24 | */ 25 | 26 | 27 | #ifndef TK_SPLINE_H 28 | #define TK_SPLINE_H 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #ifdef HAVE_SSTREAM 36 | #include 37 | #include 38 | #endif // HAVE_SSTREAM 39 | 40 | // not ideal but disable unused-function warnings 41 | // (we get them because we have implementations in the header file, 42 | // and this is because we want to be able to quickly separate them 43 | // into a cpp file if necessary) 44 | #pragma GCC diagnostic push 45 | #pragma GCC diagnostic ignored "-Wunused-function" 46 | 47 | // unnamed namespace only because the implementation is in this 48 | // header file and we don't want to export symbols to the obj files 49 | namespace 50 | { 51 | 52 | namespace tk 53 | { 54 | 55 | // spline interpolation 56 | class spline 57 | { 58 | public: 59 | // spline types 60 | enum spline_type { 61 | linear = 10, // linear interpolation 62 | cspline = 30, // cubic splines (classical C^2) 63 | cspline_hermite = 31 // cubic hermite splines (local, only C^1) 64 | }; 65 | 66 | // boundary condition type for the spline end-points 67 | enum bd_type { 68 | first_deriv = 1, 69 | second_deriv = 2 70 | }; 71 | 72 | protected: 73 | std::vector m_x,m_y; // x,y coordinates of points 74 | // interpolation parameters 75 | // f(x) = a_i + b_i*(x-x_i) + c_i*(x-x_i)^2 + d_i*(x-x_i)^3 76 | // where a_i = y_i, or else it won't go through grid points 77 | std::vector m_b,m_c,m_d; // spline coefficients 78 | double m_c0; // for left extrapolation 79 | spline_type m_type; 80 | bd_type m_left, m_right; 81 | double m_left_value, m_right_value; 82 | bool m_made_monotonic; 83 | void set_coeffs_from_b(); // calculate c_i, d_i from b_i 84 | size_t find_closest(double x) const; // closest idx so that m_x[idx]<=x 85 | 86 | public: 87 | // default constructor: set boundary condition to be zero curvature 88 | // at both ends, i.e. natural splines 89 | spline(): m_type(cspline), 90 | m_left(second_deriv), m_right(second_deriv), 91 | m_left_value(0.0), m_right_value(0.0), m_made_monotonic(false) 92 | { 93 | ; 94 | } 95 | spline(const std::vector& X, const std::vector& Y, 96 | spline_type type = cspline, 97 | bool make_monotonic = false, 98 | bd_type left = second_deriv, double left_value = 0.0, 99 | bd_type right = second_deriv, double right_value = 0.0 100 | ): 101 | m_type(type), 102 | m_left(left), m_right(right), 103 | m_left_value(left_value), m_right_value(right_value), 104 | m_made_monotonic(false) // false correct here: make_monotonic() sets it 105 | { 106 | this->set_points(X,Y,m_type); 107 | if(make_monotonic) { 108 | this->make_monotonic(); 109 | } 110 | } 111 | 112 | 113 | // modify boundary conditions: if called it must be before set_points() 114 | void set_boundary(bd_type left, double left_value, 115 | bd_type right, double right_value); 116 | 117 | // set all data points (cubic_spline=false means linear interpolation) 118 | void set_points(const std::vector& x, 119 | const std::vector& y, 120 | spline_type type=cspline); 121 | 122 | // adjust coefficients so that the spline becomes piecewise monotonic 123 | // where possible 124 | // this is done by adjusting slopes at grid points by a non-negative 125 | // factor and this will break C^2 126 | // this can also break boundary conditions if adjustments need to 127 | // be made at the boundary points 128 | // returns false if no adjustments have been made, true otherwise 129 | bool make_monotonic(); 130 | 131 | // evaluates the spline at point x 132 | double operator() (double x) const; 133 | double deriv(int order, double x) const; 134 | 135 | // returns the input data points 136 | std::vector get_x() const { return m_x; } 137 | std::vector get_y() const { return m_y; } 138 | double get_x_min() const { assert(!m_x.empty()); return m_x.front(); } 139 | double get_x_max() const { assert(!m_x.empty()); return m_x.back(); } 140 | 141 | #ifdef HAVE_SSTREAM 142 | // spline info string, i.e. spline type, boundary conditions etc. 143 | std::string info() const; 144 | #endif // HAVE_SSTREAM 145 | 146 | }; 147 | 148 | 149 | 150 | namespace internal 151 | { 152 | 153 | // band matrix solver 154 | class band_matrix 155 | { 156 | private: 157 | std::vector< std::vector > m_upper; // upper band 158 | std::vector< std::vector > m_lower; // lower band 159 | public: 160 | band_matrix() {}; // constructor 161 | band_matrix(int dim, int n_u, int n_l); // constructor 162 | ~band_matrix() {}; // destructor 163 | void resize(int dim, int n_u, int n_l); // init with dim,n_u,n_l 164 | int dim() const; // matrix dimension 165 | int num_upper() const 166 | { 167 | return (int)m_upper.size()-1; 168 | } 169 | int num_lower() const 170 | { 171 | return (int)m_lower.size()-1; 172 | } 173 | // access operator 174 | double & operator () (int i, int j); // write 175 | double operator () (int i, int j) const; // read 176 | // we can store an additional diagonal (in m_lower) 177 | double& saved_diag(int i); 178 | double saved_diag(int i) const; 179 | void lu_decompose(); 180 | std::vector r_solve(const std::vector& b) const; 181 | std::vector l_solve(const std::vector& b) const; 182 | std::vector lu_solve(const std::vector& b, 183 | bool is_lu_decomposed=false); 184 | 185 | }; 186 | 187 | } // namespace internal 188 | 189 | 190 | 191 | 192 | // --------------------------------------------------------------------- 193 | // implementation part, which could be separated into a cpp file 194 | // --------------------------------------------------------------------- 195 | 196 | // spline implementation 197 | // ----------------------- 198 | 199 | void spline::set_boundary(spline::bd_type left, double left_value, 200 | spline::bd_type right, double right_value) 201 | { 202 | assert(m_x.size()==0); // set_points() must not have happened yet 203 | m_left=left; 204 | m_right=right; 205 | m_left_value=left_value; 206 | m_right_value=right_value; 207 | } 208 | 209 | 210 | void spline::set_coeffs_from_b() 211 | { 212 | assert(m_x.size()==m_y.size()); 213 | assert(m_x.size()==m_b.size()); 214 | assert(m_x.size()>2); 215 | size_t n=m_b.size(); 216 | if(m_c.size()!=n) 217 | m_c.resize(n); 218 | if(m_d.size()!=n) 219 | m_d.resize(n); 220 | 221 | for(size_t i=0; i& x, 234 | const std::vector& y, 235 | spline_type type) 236 | { 237 | assert(x.size()==y.size()); 238 | assert(x.size()>2); 239 | m_type=type; 240 | m_made_monotonic=false; 241 | m_x=x; 242 | m_y=y; 243 | int n = (int) x.size(); 244 | // check strict monotonicity of input vector x 245 | for(int i=0; i rhs(n); 272 | for(int i=1; i2); 380 | bool modified = false; 381 | const int n=(int)m_x.size(); 382 | // make sure: input data monotonic increasing --> b_i>=0 383 | // input data monotonic decreasing --> b_i<=0 384 | for(int i=0; i=m_y[i]) && (m_y[i]>=m_y[ip1]) && m_b[i]>0.0) ) { 389 | modified=true; 390 | m_b[i]=0.0; 391 | } 392 | } 393 | // if input data is monotonic (b[i], b[i+1], avg have all the same sign) 394 | // ensure a sufficient criteria for monotonicity is satisfied: 395 | // sqrt(b[i]^2+b[i+1]^2) <= 3 |avg|, with avg=(y[i+1]-y[i])/h, 396 | for(int i=0; i=0.0 && m_b[i+1]>=0.0 && avg>0.0) || 404 | (m_b[i]<=0.0 && m_b[i+1]<=0.0 && avg<0.0) ) { 405 | // input data is monotonic 406 | double r = sqrt(m_b[i]*m_b[i]+m_b[i+1]*m_b[i+1])/std::fabs(avg); 407 | if(r>3.0) { 408 | // sufficient criteria for monotonicity: r<=3 409 | // adjust b[i] and b[i+1] 410 | modified=true; 411 | m_b[i] *= (3.0/r); 412 | m_b[i+1] *= (3.0/r); 413 | } 414 | } 415 | } 416 | 417 | if(modified==true) { 418 | set_coeffs_from_b(); 419 | m_made_monotonic=true; 420 | } 421 | 422 | return modified; 423 | } 424 | 425 | // return the closest idx so that m_x[idx] <= x (return 0 if x::const_iterator it; 429 | it=std::upper_bound(m_x.begin(),m_x.end(),x); // *it > x 430 | size_t idx = std::max( int(it-m_x.begin())-1, 0); // m_x[idx] <= x 431 | return idx; 432 | } 433 | 434 | double spline::operator() (double x) const 435 | { 436 | // polynomial evaluation using Horner's scheme 437 | // TODO: consider more numerically accurate algorithms, e.g.: 438 | // - Clenshaw 439 | // - Even-Odd method by A.C.R. Newbery 440 | // - Compensated Horner Scheme 441 | size_t n=m_x.size(); 442 | size_t idx=find_closest(x); 443 | 444 | double h=x-m_x[idx]; 445 | double interpol; 446 | if(xm_x[n-1]) { 450 | // extrapolation to the right 451 | interpol=(m_c[n-1]*h + m_b[n-1])*h + m_y[n-1]; 452 | } else { 453 | // interpolation 454 | interpol=((m_d[idx]*h + m_c[idx])*h + m_b[idx])*h + m_y[idx]; 455 | } 456 | return interpol; 457 | } 458 | 459 | double spline::deriv(int order, double x) const 460 | { 461 | assert(order>0); 462 | size_t n=m_x.size(); 463 | size_t idx = find_closest(x); 464 | 465 | double h=x-m_x[idx]; 466 | double interpol; 467 | if(xm_x[n-1]) { 481 | // extrapolation to the right 482 | switch(order) { 483 | case 1: 484 | interpol=2.0*m_c[n-1]*h + m_b[n-1]; 485 | break; 486 | case 2: 487 | interpol=2.0*m_c[n-1]; 488 | break; 489 | default: 490 | interpol=0.0; 491 | break; 492 | } 493 | } else { 494 | // interpolation 495 | switch(order) { 496 | case 1: 497 | interpol=(3.0*m_d[idx]*h + 2.0*m_c[idx])*h + m_b[idx]; 498 | break; 499 | case 2: 500 | interpol=6.0*m_d[idx]*h + 2.0*m_c[idx]; 501 | break; 502 | case 3: 503 | interpol=6.0*m_d[idx]; 504 | break; 505 | default: 506 | interpol=0.0; 507 | break; 508 | } 509 | } 510 | return interpol; 511 | } 512 | 513 | #ifdef HAVE_SSTREAM 514 | std::string spline::info() const 515 | { 516 | std::stringstream ss; 517 | ss << "type " << m_type << ", left boundary deriv " << m_left << " = "; 518 | ss << m_left_value << ", right boundary deriv " << m_right << " = "; 519 | ss << m_right_value << std::endl; 520 | if(m_made_monotonic) { 521 | ss << "(spline has been adjusted for piece-wise monotonicity)"; 522 | } 523 | return ss.str(); 524 | } 525 | #endif // HAVE_SSTREAM 526 | 527 | 528 | namespace internal 529 | { 530 | 531 | // band_matrix implementation 532 | // ------------------------- 533 | 534 | band_matrix::band_matrix(int dim, int n_u, int n_l) 535 | { 536 | resize(dim, n_u, n_l); 537 | } 538 | void band_matrix::resize(int dim, int n_u, int n_l) 539 | { 540 | assert(dim>0); 541 | assert(n_u>=0); 542 | assert(n_l>=0); 543 | m_upper.resize(n_u+1); 544 | m_lower.resize(n_l+1); 545 | for(size_t i=0; i0) { 555 | return m_upper[0].size(); 556 | } else { 557 | return 0; 558 | } 559 | } 560 | 561 | 562 | // defines the new operator (), so that we can access the elements 563 | // by A(i,j), index going from i=0,...,dim()-1 564 | double & band_matrix::operator () (int i, int j) 565 | { 566 | int k=j-i; // what band is the entry 567 | assert( (i>=0) && (i=0) && (j diagonal, k<0 lower left part, k>0 upper right part 570 | if(k>=0) return m_upper[k][i]; 571 | else return m_lower[-k][i]; 572 | } 573 | double band_matrix::operator () (int i, int j) const 574 | { 575 | int k=j-i; // what band is the entry 576 | assert( (i>=0) && (i=0) && (j diagonal, k<0 lower left part, k>0 upper right part 579 | if(k>=0) return m_upper[k][i]; 580 | else return m_lower[-k][i]; 581 | } 582 | // second diag (used in LU decomposition), saved in m_lower 583 | double band_matrix::saved_diag(int i) const 584 | { 585 | assert( (i>=0) && (i=0) && (idim(); i++) { 604 | assert(this->operator()(i,i)!=0.0); 605 | this->saved_diag(i)=1.0/this->operator()(i,i); 606 | j_min=std::max(0,i-this->num_lower()); 607 | j_max=std::min(this->dim()-1,i+this->num_upper()); 608 | for(int j=j_min; j<=j_max; j++) { 609 | this->operator()(i,j) *= this->saved_diag(i); 610 | } 611 | this->operator()(i,i)=1.0; // prevents rounding errors 612 | } 613 | 614 | // Gauss LR-Decomposition 615 | for(int k=0; kdim(); k++) { 616 | i_max=std::min(this->dim()-1,k+this->num_lower()); // num_lower not a mistake! 617 | for(int i=k+1; i<=i_max; i++) { 618 | assert(this->operator()(k,k)!=0.0); 619 | x=-this->operator()(i,k)/this->operator()(k,k); 620 | this->operator()(i,k)=-x; // assembly part of L 621 | j_max=std::min(this->dim()-1,k+this->num_upper()); 622 | for(int j=k+1; j<=j_max; j++) { 623 | // assembly part of R 624 | this->operator()(i,j)=this->operator()(i,j)+x*this->operator()(k,j); 625 | } 626 | } 627 | } 628 | } 629 | // solves Ly=b 630 | std::vector band_matrix::l_solve(const std::vector& b) const 631 | { 632 | assert( this->dim()==(int)b.size() ); 633 | std::vector x(this->dim()); 634 | int j_start; 635 | double sum; 636 | for(int i=0; idim(); i++) { 637 | sum=0; 638 | j_start=std::max(0,i-this->num_lower()); 639 | for(int j=j_start; joperator()(i,j)*x[j]; 640 | x[i]=(b[i]*this->saved_diag(i)) - sum; 641 | } 642 | return x; 643 | } 644 | // solves Rx=y 645 | std::vector band_matrix::r_solve(const std::vector& b) const 646 | { 647 | assert( this->dim()==(int)b.size() ); 648 | std::vector x(this->dim()); 649 | int j_stop; 650 | double sum; 651 | for(int i=this->dim()-1; i>=0; i--) { 652 | sum=0; 653 | j_stop=std::min(this->dim()-1,i+this->num_upper()); 654 | for(int j=i+1; j<=j_stop; j++) sum += this->operator()(i,j)*x[j]; 655 | x[i]=( b[i] - sum ) / this->operator()(i,i); 656 | } 657 | return x; 658 | } 659 | 660 | std::vector band_matrix::lu_solve(const std::vector& b, 661 | bool is_lu_decomposed) 662 | { 663 | assert( this->dim()==(int)b.size() ); 664 | std::vector x,y; 665 | if(is_lu_decomposed==false) { 666 | this->lu_decompose(); 667 | } 668 | y=this->l_solve(b); 669 | x=this->r_solve(y); 670 | return x; 671 | } 672 | 673 | } // namespace internal 674 | 675 | 676 | } // namespace tk 677 | 678 | 679 | } // namespace 680 | 681 | #pragma GCC diagnostic pop 682 | 683 | #endif /* TK_SPLINE_H */ --------------------------------------------------------------------------------