├── .gitignore ├── README.md ├── amcl ├── CHANGELOG.rst ├── CMakeLists.txt ├── cfg │ └── AMCL.cfg ├── examples │ ├── amcl_diff.launch │ └── amcl_omni.launch ├── include │ └── amcl │ │ ├── map │ │ └── map.h │ │ ├── pf │ │ ├── eig3.h │ │ ├── pf.h │ │ ├── pf_kdtree.h │ │ ├── pf_pdf.h │ │ └── pf_vector.h │ │ └── sensors │ │ ├── amcl_laser.h │ │ ├── amcl_odom.h │ │ └── amcl_sensor.h ├── package.xml ├── src │ ├── amcl │ │ ├── map │ │ │ ├── map.c │ │ │ ├── map_cspace.cpp │ │ │ ├── map_draw.c │ │ │ ├── map_range.c │ │ │ └── map_store.c │ │ ├── pf │ │ │ ├── eig3.c │ │ │ ├── pf.c │ │ │ ├── pf_draw.c │ │ │ ├── pf_kdtree.c │ │ │ ├── pf_pdf.c │ │ │ └── pf_vector.c │ │ └── sensors │ │ │ ├── amcl_laser.cpp │ │ │ ├── amcl_odom.cpp │ │ │ └── amcl_sensor.cpp │ ├── amcl_node.cpp │ └── include │ │ └── portable_utils.hpp └── test │ ├── basic_localization.py │ ├── basic_localization_stage.xml │ ├── global_localization_stage.xml │ ├── rosie_multilaser.xml │ ├── set_initial_pose.xml │ ├── set_initial_pose_delayed.xml │ ├── set_pose.py │ ├── small_loop_crazy_driving_prg.xml │ ├── small_loop_crazy_driving_prg_corrected.xml │ ├── small_loop_prf.xml │ ├── texas_greenroom_loop.xml │ ├── texas_greenroom_loop_corrected.xml │ ├── texas_willow_hallway_loop.xml │ └── texas_willow_hallway_loop_corrected.xml ├── base_local_planner ├── CHANGELOG.rst ├── CMakeLists.txt ├── blp_plugin.xml ├── cfg │ ├── BaseLocalPlanner.cfg │ └── LocalPlannerLimits.cfg ├── include │ └── base_local_planner │ │ ├── costmap_model.h │ │ ├── footprint_helper.h │ │ ├── goal_functions.h │ │ ├── latched_stop_rotate_controller.h │ │ ├── line_iterator.h │ │ ├── local_planner_limits.h │ │ ├── local_planner_util.h │ │ ├── map_cell.h │ │ ├── map_grid.h │ │ ├── map_grid_cost_function.h │ │ ├── map_grid_visualizer.h │ │ ├── obstacle_cost_function.h │ │ ├── odometry_helper_ros.h │ │ ├── oscillation_cost_function.h │ │ ├── planar_laser_scan.h │ │ ├── point_grid.h │ │ ├── prefer_forward_cost_function.h │ │ ├── simple_scored_sampling_planner.h │ │ ├── simple_trajectory_generator.h │ │ ├── trajectory.h │ │ ├── trajectory_cost_function.h │ │ ├── trajectory_inc.h │ │ ├── trajectory_planner.h │ │ ├── trajectory_planner_ros.h │ │ ├── trajectory_sample_generator.h │ │ ├── trajectory_search.h │ │ ├── twirling_cost_function.h │ │ ├── velocity_iterator.h │ │ ├── voxel_grid_model.h │ │ └── world_model.h ├── msg │ └── Position2DInt.msg ├── package.xml ├── setup.py ├── src │ ├── costmap_model.cpp │ ├── footprint_helper.cpp │ ├── goal_functions.cpp │ ├── latched_stop_rotate_controller.cpp │ ├── local_planner_limits │ │ ├── .gitignore │ │ └── __init__.py │ ├── local_planner_util.cpp │ ├── map_cell.cpp │ ├── map_grid.cpp │ ├── map_grid_cost_function.cpp │ ├── map_grid_visualizer.cpp │ ├── obstacle_cost_function.cpp │ ├── odometry_helper_ros.cpp │ ├── oscillation_cost_function.cpp │ ├── point_grid.cpp │ ├── point_grid_node.cpp │ ├── prefer_forward_cost_function.cpp │ ├── simple_scored_sampling_planner.cpp │ ├── simple_trajectory_generator.cpp │ ├── trajectory.cpp │ ├── trajectory_planner.cpp │ ├── trajectory_planner_ros.cpp │ ├── twirling_cost_function.cpp │ └── voxel_grid_model.cpp └── test │ ├── footprint_helper_test.cpp │ ├── gtest_main.cpp │ ├── line_iterator_test.cpp │ ├── map_grid_test.cpp │ ├── trajectory_generator_test.cpp │ ├── utest.cpp │ ├── velocity_iterator_test.cpp │ └── wavefront_map_accessor.h ├── carrot_planner ├── CHANGELOG.rst ├── CMakeLists.txt ├── bgp_plugin.xml ├── include │ └── carrot_planner │ │ └── carrot_planner.h ├── package.xml └── src │ └── carrot_planner.cpp ├── clear_costmap_recovery ├── CHANGELOG.rst ├── CMakeLists.txt ├── ccr_plugin.xml ├── include │ └── clear_costmap_recovery │ │ └── clear_costmap_recovery.h ├── package.xml ├── src │ └── clear_costmap_recovery.cpp └── test │ ├── clear_tester.cpp │ ├── clear_tests.launch │ └── params.yaml ├── costmap_2d ├── CHANGELOG.rst ├── CMakeLists.txt ├── cfg │ ├── Costmap2D.cfg │ ├── GenericPlugin.cfg │ ├── InflationPlugin.cfg │ ├── ObstaclePlugin.cfg │ └── VoxelPlugin.cfg ├── costmap_plugins.xml ├── include │ └── costmap_2d │ │ ├── array_parser.h │ │ ├── cost_values.h │ │ ├── costmap_2d.h │ │ ├── costmap_2d_publisher.h │ │ ├── costmap_2d_ros.h │ │ ├── costmap_layer.h │ │ ├── costmap_math.h │ │ ├── footprint.h │ │ ├── inflation_layer.h │ │ ├── layer.h │ │ ├── layered_costmap.h │ │ ├── observation.h │ │ ├── observation_buffer.h │ │ ├── obstacle_layer.h │ │ ├── static_layer.h │ │ ├── testing_helper.h │ │ └── voxel_layer.h ├── launch │ ├── example.launch │ └── example_params.yaml ├── msg │ └── VoxelGrid.msg ├── package.xml ├── plugins │ ├── inflation_layer.cpp │ ├── obstacle_layer.cpp │ ├── static_layer.cpp │ └── voxel_layer.cpp ├── src │ ├── array_parser.cpp │ ├── costmap_2d.cpp │ ├── costmap_2d_cloud.cpp │ ├── costmap_2d_markers.cpp │ ├── costmap_2d_node.cpp │ ├── costmap_2d_publisher.cpp │ ├── costmap_2d_ros.cpp │ ├── costmap_layer.cpp │ ├── costmap_math.cpp │ ├── footprint.cpp │ ├── layer.cpp │ ├── layered_costmap.cpp │ └── observation_buffer.cpp └── test │ ├── TenByTen.pgm │ ├── TenByTen.yaml │ ├── array_parser_test.cpp │ ├── coordinates_test.cpp │ ├── costmap_params.yaml │ ├── costmap_tester.cpp │ ├── footprint_tests.cpp │ ├── footprint_tests.launch │ ├── inflation_tests.cpp │ ├── inflation_tests.launch │ ├── module_tests.cpp │ ├── obstacle_tests.cpp │ ├── obstacle_tests.launch │ ├── simple_driving_test.xml │ ├── static_tests.cpp │ └── static_tests.launch ├── dwa_local_planner ├── CHANGELOG.rst ├── CMakeLists.txt ├── blp_plugin.xml ├── cfg │ └── DWAPlanner.cfg ├── include │ └── dwa_local_planner │ │ ├── dwa_planner.h │ │ └── dwa_planner_ros.h ├── package.xml └── src │ ├── dwa_planner.cpp │ └── dwa_planner_ros.cpp ├── fake_localization ├── CHANGELOG.rst ├── CMakeLists.txt ├── fake_localization.cpp ├── package.xml └── static_odom_broadcaster.py ├── global_planner ├── CHANGELOG.rst ├── CMakeLists.txt ├── bgp_plugin.xml ├── cfg │ └── GlobalPlanner.cfg ├── include │ └── global_planner │ │ ├── astar.h │ │ ├── dijkstra.h │ │ ├── expander.h │ │ ├── gradient_path.h │ │ ├── grid_path.h │ │ ├── orientation_filter.h │ │ ├── planner_core.h │ │ ├── potential_calculator.h │ │ ├── quadratic_calculator.h │ │ └── traceback.h ├── package.xml └── src │ ├── astar.cpp │ ├── dijkstra.cpp │ ├── gradient_path.cpp │ ├── grid_path.cpp │ ├── orientation_filter.cpp │ ├── plan_node.cpp │ ├── planner_core.cpp │ └── quadratic_calculator.cpp ├── map_server ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── map_server │ │ └── image_loader.h ├── package.xml ├── scripts │ └── crop_map ├── src │ ├── image_loader.cpp │ ├── main.cpp │ ├── map_saver.cpp │ └── map_server.dox └── test │ ├── consumer.py │ ├── rtest.cpp │ ├── rtest.xml │ ├── spectrum.png │ ├── test_constants.cpp │ ├── test_constants.h │ ├── testmap.bmp │ ├── testmap.png │ ├── testmap.yaml │ ├── testmap2.png │ ├── testmap2.yaml │ └── utest.cpp ├── move_base ├── CHANGELOG.rst ├── CMakeLists.txt ├── cfg │ └── MoveBase.cfg ├── include │ └── move_base │ │ └── move_base.h ├── package.xml ├── planner_test.xml └── src │ ├── move_base.cpp │ └── move_base_node.cpp ├── move_slow_and_clear ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── move_slow_and_clear │ │ └── move_slow_and_clear.h ├── package.xml ├── recovery_plugin.xml └── src │ └── move_slow_and_clear.cpp ├── nav_core ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── nav_core │ │ ├── base_global_planner.h │ │ ├── base_local_planner.h │ │ ├── parameter_magic.h │ │ └── recovery_behavior.h └── package.xml ├── navfn ├── CHANGELOG.rst ├── CMakeLists.txt ├── Makefile.orig ├── bgp_plugin.xml ├── include │ └── navfn │ │ ├── navfn.h │ │ ├── navfn_ros.h │ │ ├── potarr_point.h │ │ └── read_pgm_costmap.h ├── package.xml ├── src │ ├── navfn.cpp │ ├── navfn_node.cpp │ ├── navfn_ros.cpp │ ├── navtest │ │ ├── navtest.cpp │ │ ├── navwin.cpp │ │ └── navwin.h │ └── read_pgm_costmap.cpp ├── srv │ ├── MakeNavPlan.srv │ └── SetCostmap.srv └── test │ ├── CMakeLists.txt │ ├── path_calc_test.cpp │ └── willow_costmap.pgm ├── navigation ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.rst └── package.xml ├── rotate_recovery ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── rotate_recovery │ │ └── rotate_recovery.h ├── package.xml ├── rotate_plugin.xml └── src │ └── rotate_recovery.cpp └── voxel_grid ├── CHANGELOG.rst ├── CMakeLists.txt ├── include └── voxel_grid │ └── voxel_grid.h ├── package.xml ├── src └── voxel_grid.cpp └── test └── voxel_grid_tests.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.project 3 | *.cproject 4 | 5 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ROS Navigation Stack 2 | ==================== 3 | 4 | A 2D navigation stack that takes in information from odometry, sensor 5 | streams, and a goal pose and outputs safe velocity commands that are sent 6 | to a mobile base. 7 | 8 | * AMD64 Debian Job Status: [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__navigation__ubuntu_bionic_amd64__binary)](http://build.ros.org/job/Mbin_uB64__navigation__ubuntu_bionic_amd64__binary/) 9 | 10 | Related stacks: 11 | 12 | * http://github.com/ros-planning/navigation_msgs (new in Jade+) 13 | * http://github.com/ros-planning/navigation_tutorials 14 | * http://github.com/ros-planning/navigation_experimental 15 | 16 | For discussion, please check out the 17 | https://groups.google.com/group/ros-sig-navigation mailing list. 18 | -------------------------------------------------------------------------------- /amcl/examples/amcl_diff.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 | 34 | 35 | -------------------------------------------------------------------------------- /amcl/examples/amcl_omni.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 | 34 | 35 | -------------------------------------------------------------------------------- /amcl/include/amcl/pf/eig3.h: -------------------------------------------------------------------------------- 1 | 2 | /* Eigen-decomposition for symmetric 3x3 real matrices. 3 | Public domain, copied from the public domain Java library JAMA. */ 4 | 5 | #ifndef _eig_h 6 | 7 | /* Symmetric matrix A => eigenvectors in columns of V, corresponding 8 | eigenvalues in d. */ 9 | void eigen_decomposition(double A[3][3], double V[3][3], double d[3]); 10 | 11 | #endif 12 | -------------------------------------------------------------------------------- /amcl/include/amcl/pf/pf_pdf.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Useful pdf functions 23 | * Author: Andrew Howard 24 | * Date: 10 Dec 2002 25 | * CVS: $Id: pf_pdf.h 6345 2008-04-17 01:36:39Z gerkey $ 26 | *************************************************************************/ 27 | 28 | #ifndef PF_PDF_H 29 | #define PF_PDF_H 30 | 31 | #include "pf_vector.h" 32 | 33 | //#include 34 | //#include 35 | 36 | #ifdef __cplusplus 37 | extern "C" { 38 | #endif 39 | 40 | /************************************************************************** 41 | * Gaussian 42 | *************************************************************************/ 43 | 44 | // Gaussian PDF info 45 | typedef struct 46 | { 47 | // Mean, covariance and inverse covariance 48 | pf_vector_t x; 49 | pf_matrix_t cx; 50 | //pf_matrix_t cxi; 51 | double cxdet; 52 | 53 | // Decomposed covariance matrix (rotation * diagonal) 54 | pf_matrix_t cr; 55 | pf_vector_t cd; 56 | 57 | // A random number generator 58 | //gsl_rng *rng; 59 | 60 | } pf_pdf_gaussian_t; 61 | 62 | 63 | // Create a gaussian pdf 64 | pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx); 65 | 66 | // Destroy the pdf 67 | void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf); 68 | 69 | // Compute the value of the pdf at some point [z]. 70 | //double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t z); 71 | 72 | // Draw randomly from a zero-mean Gaussian distribution, with standard 73 | // deviation sigma. 74 | // We use the polar form of the Box-Muller transformation, explained here: 75 | // http://www.taygeta.com/random/gaussian.html 76 | double pf_ran_gaussian(double sigma); 77 | 78 | // Generate a sample from the pdf. 79 | pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf); 80 | 81 | #ifdef __cplusplus 82 | } 83 | #endif 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /amcl/include/amcl/pf/pf_vector.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Vector functions 23 | * Author: Andrew Howard 24 | * Date: 10 Dec 2002 25 | * CVS: $Id: pf_vector.h 6345 2008-04-17 01:36:39Z gerkey $ 26 | *************************************************************************/ 27 | 28 | #ifndef PF_VECTOR_H 29 | #define PF_VECTOR_H 30 | 31 | #ifdef __cplusplus 32 | extern "C" { 33 | #endif 34 | 35 | #include 36 | 37 | // The basic vector 38 | typedef struct 39 | { 40 | double v[3]; 41 | } pf_vector_t; 42 | 43 | 44 | // The basic matrix 45 | typedef struct 46 | { 47 | double m[3][3]; 48 | } pf_matrix_t; 49 | 50 | 51 | // Return a zero vector 52 | pf_vector_t pf_vector_zero(); 53 | 54 | // Check for NAN or INF in any component 55 | int pf_vector_finite(pf_vector_t a); 56 | 57 | // Print a vector 58 | void pf_vector_fprintf(pf_vector_t s, FILE *file, const char *fmt); 59 | 60 | // Simple vector addition 61 | pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b); 62 | 63 | // Simple vector subtraction 64 | pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b); 65 | 66 | // Transform from local to global coords (a + b) 67 | pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b); 68 | 69 | // Transform from global to local coords (a - b) 70 | pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b); 71 | 72 | 73 | // Return a zero matrix 74 | pf_matrix_t pf_matrix_zero(); 75 | 76 | // Check for NAN or INF in any component 77 | int pf_matrix_finite(pf_matrix_t a); 78 | 79 | // Print a matrix 80 | void pf_matrix_fprintf(pf_matrix_t s, FILE *file, const char *fmt); 81 | 82 | // Compute the matrix inverse. Will also return the determinant, 83 | // which should be checked for underflow (indicated singular matrix). 84 | //pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det); 85 | 86 | // Decompose a covariance matrix [a] into a rotation matrix [r] and a 87 | // diagonal matrix [d] such that a = r * d * r^T. 88 | void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a); 89 | 90 | #ifdef __cplusplus 91 | } 92 | #endif 93 | 94 | #endif 95 | -------------------------------------------------------------------------------- /amcl/include/amcl/sensors/amcl_odom.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey et al. 4 | * 5 | * This library is free software; you can redistribute it and/or 6 | * modify it under the terms of the GNU Lesser General Public 7 | * License as published by the Free Software Foundation; either 8 | * version 2.1 of the License, or (at your option) any later version. 9 | * 10 | * This library is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | * Lesser General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU Lesser General Public 16 | * License along with this library; if not, write to the Free Software 17 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 18 | * 19 | */ 20 | /////////////////////////////////////////////////////////////////////////// 21 | // 22 | // Desc: Odometry sensor model for AMCL 23 | // Author: Andrew Howard 24 | // Date: 17 Aug 2003 25 | // CVS: $Id: amcl_odom.h 4135 2007-08-23 19:58:48Z gerkey $ 26 | // 27 | /////////////////////////////////////////////////////////////////////////// 28 | 29 | #ifndef AMCL_ODOM_H 30 | #define AMCL_ODOM_H 31 | 32 | #include "amcl_sensor.h" 33 | #include "../pf/pf_pdf.h" 34 | 35 | namespace amcl 36 | { 37 | 38 | typedef enum 39 | { 40 | ODOM_MODEL_DIFF, 41 | ODOM_MODEL_OMNI, 42 | ODOM_MODEL_DIFF_CORRECTED, 43 | ODOM_MODEL_OMNI_CORRECTED 44 | } odom_model_t; 45 | 46 | // Odometric sensor data 47 | class AMCLOdomData : public AMCLSensorData 48 | { 49 | // Odometric pose 50 | public: pf_vector_t pose; 51 | 52 | // Change in odometric pose 53 | public: pf_vector_t delta; 54 | }; 55 | 56 | 57 | // Odometric sensor model 58 | class AMCLOdom : public AMCLSensor 59 | { 60 | // Default constructor 61 | public: AMCLOdom(); 62 | 63 | public: void SetModelDiff(double alpha1, 64 | double alpha2, 65 | double alpha3, 66 | double alpha4); 67 | 68 | public: void SetModelOmni(double alpha1, 69 | double alpha2, 70 | double alpha3, 71 | double alpha4, 72 | double alpha5); 73 | 74 | public: void SetModel( odom_model_t type, 75 | double alpha1, 76 | double alpha2, 77 | double alpha3, 78 | double alpha4, 79 | double alpha5 = 0 ); 80 | 81 | // Update the filter based on the action model. Returns true if the filter 82 | // has been updated. 83 | public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data); 84 | 85 | // Current data timestamp 86 | private: double time; 87 | 88 | // Model type 89 | private: odom_model_t model_type; 90 | 91 | // Drift parameters 92 | private: double alpha1, alpha2, alpha3, alpha4, alpha5; 93 | }; 94 | 95 | 96 | } 97 | 98 | #endif 99 | -------------------------------------------------------------------------------- /amcl/include/amcl/sensors/amcl_sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey et al. 4 | * 5 | * This library is free software; you can redistribute it and/or 6 | * modify it under the terms of the GNU Lesser General Public 7 | * License as published by the Free Software Foundation; either 8 | * version 2.1 of the License, or (at your option) any later version. 9 | * 10 | * This library is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | * Lesser General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU Lesser General Public 16 | * License along with this library; if not, write to the Free Software 17 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 18 | */ 19 | /////////////////////////////////////////////////////////////////////////// 20 | // 21 | // Desc: Adaptive Monte-Carlo localization 22 | // Author: Andrew Howard 23 | // Date: 6 Feb 2003 24 | // CVS: $Id: amcl_sensor.h 6443 2008-05-15 19:46:11Z gerkey $ 25 | // 26 | /////////////////////////////////////////////////////////////////////////// 27 | 28 | #ifndef AMCL_SENSOR_H 29 | #define AMCL_SENSOR_H 30 | 31 | #include "../pf/pf.h" 32 | 33 | namespace amcl 34 | { 35 | 36 | // Forward declarations 37 | class AMCLSensorData; 38 | 39 | 40 | // Base class for all AMCL sensors 41 | class AMCLSensor 42 | { 43 | // Default constructor 44 | public: AMCLSensor(); 45 | 46 | // Default destructor 47 | public: virtual ~AMCLSensor(); 48 | 49 | // Update the filter based on the action model. Returns true if the filter 50 | // has been updated. 51 | public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data); 52 | 53 | // Initialize the filter based on the sensor model. Returns true if the 54 | // filter has been initialized. 55 | public: virtual bool InitSensor(pf_t *pf, AMCLSensorData *data); 56 | 57 | // Update the filter based on the sensor model. Returns true if the 58 | // filter has been updated. 59 | public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data); 60 | 61 | // Flag is true if this is the action sensor 62 | public: bool is_action; 63 | 64 | // Action pose (action sensors only) 65 | public: pf_vector_t pose; 66 | 67 | // AMCL Base 68 | //protected: AdaptiveMCL & AMCL; 69 | 70 | #ifdef INCLUDE_RTKGUI 71 | // Setup the GUI 72 | public: virtual void SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig); 73 | 74 | // Finalize the GUI 75 | public: virtual void ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig); 76 | 77 | // Draw sensor data 78 | public: virtual void UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data); 79 | #endif 80 | }; 81 | 82 | 83 | 84 | // Base class for all AMCL sensor measurements 85 | class AMCLSensorData 86 | { 87 | // Pointer to sensor that generated the data 88 | public: AMCLSensor *sensor; 89 | virtual ~AMCLSensorData() {} 90 | 91 | // Data timestamp 92 | public: double time; 93 | }; 94 | 95 | } 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /amcl/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | amcl 5 | 1.17.1 6 | 7 |

8 | amcl is a probabilistic localization system for a robot moving in 9 | 2D. It implements the adaptive (or KLD-sampling) Monte Carlo 10 | localization approach (as described by Dieter Fox), which uses a 11 | particle filter to track the pose of a robot against a known map. 12 |

13 |

14 | This node is derived, with thanks, from Andrew Howard's excellent 15 | 'amcl' Player driver. 16 |

17 | 18 | http://wiki.ros.org/amcl 19 | Brian P. Gerkey 20 | contradict@gmail.com 21 | David V. Lu!! 22 | Michael Ferguson 23 | Aaron Hoy 24 | LGPL 25 | 26 | catkin 27 | 28 | message_filters 29 | tf2_geometry_msgs 30 | 31 | diagnostic_updater 32 | dynamic_reconfigure 33 | geometry_msgs 34 | nav_msgs 35 | rosbag 36 | roscpp 37 | sensor_msgs 38 | std_srvs 39 | tf2 40 | tf2_msgs 41 | tf2_ros 42 | 43 | map_server 44 | rostest 45 | python3-pykdl 46 | tf2_py 47 | 48 | -------------------------------------------------------------------------------- /amcl/src/amcl/map/map.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Global map (grid-based) 23 | * Author: Andrew Howard 24 | * Date: 6 Feb 2003 25 | * CVS: $Id: map.c 1713 2003-08-23 04:03:43Z inspectorg $ 26 | **************************************************************************/ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "amcl/map/map.h" 35 | 36 | 37 | // Create a new map 38 | map_t *map_alloc(void) 39 | { 40 | map_t *map; 41 | 42 | map = (map_t*) malloc(sizeof(map_t)); 43 | 44 | // Assume we start at (0, 0) 45 | map->origin_x = 0; 46 | map->origin_y = 0; 47 | 48 | // Make the size odd 49 | map->size_x = 0; 50 | map->size_y = 0; 51 | map->scale = 0; 52 | 53 | // Allocate storage for main map 54 | map->cells = (map_cell_t*) NULL; 55 | 56 | return map; 57 | } 58 | 59 | 60 | // Destroy a map 61 | void map_free(map_t *map) 62 | { 63 | free(map->cells); 64 | free(map); 65 | return; 66 | } 67 | 68 | 69 | // Get the cell at the given point 70 | map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa) 71 | { 72 | int i, j; 73 | map_cell_t *cell; 74 | 75 | i = MAP_GXWX(map, ox); 76 | j = MAP_GYWY(map, oy); 77 | 78 | if (!MAP_VALID(map, i, j)) 79 | return NULL; 80 | 81 | cell = map->cells + MAP_INDEX(map, i, j); 82 | return cell; 83 | } 84 | 85 | -------------------------------------------------------------------------------- /amcl/src/amcl/sensors/amcl_sensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /////////////////////////////////////////////////////////////////////////// 22 | // 23 | // Desc: AMCL sensor 24 | // Author: Andrew Howard 25 | // Date: 6 Feb 2003 26 | // CVS: $Id: amcl_sensor.cc 7057 2008-10-02 00:44:06Z gbiggs $ 27 | // 28 | /////////////////////////////////////////////////////////////////////////// 29 | 30 | 31 | #include "amcl/sensors/amcl_sensor.h" 32 | 33 | using namespace amcl; 34 | 35 | //////////////////////////////////////////////////////////////////////////////// 36 | // Default constructor 37 | AMCLSensor::AMCLSensor() 38 | { 39 | return; 40 | } 41 | 42 | AMCLSensor::~AMCLSensor() 43 | { 44 | } 45 | 46 | //////////////////////////////////////////////////////////////////////////////// 47 | // Apply the action model 48 | bool AMCLSensor::UpdateAction(pf_t *pf, AMCLSensorData *data) 49 | { 50 | return false; 51 | } 52 | 53 | 54 | //////////////////////////////////////////////////////////////////////////////// 55 | // Initialize the filter 56 | bool AMCLSensor::InitSensor(pf_t *pf, AMCLSensorData *data) 57 | { 58 | return false; 59 | } 60 | 61 | 62 | //////////////////////////////////////////////////////////////////////////////// 63 | // Apply the sensor model 64 | bool AMCLSensor::UpdateSensor(pf_t *pf, AMCLSensorData *data) 65 | { 66 | return false; 67 | } 68 | 69 | 70 | #ifdef INCLUDE_RTKGUI 71 | 72 | //////////////////////////////////////////////////////////////////////////////// 73 | // Setup the GUI 74 | void AMCLSensor::SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) 75 | { 76 | return; 77 | } 78 | 79 | 80 | //////////////////////////////////////////////////////////////////////////////// 81 | // Shutdown the GUI 82 | void AMCLSensor::ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) 83 | { 84 | return; 85 | } 86 | 87 | 88 | //////////////////////////////////////////////////////////////////////////////// 89 | // Draw sensor data 90 | void AMCLSensor::UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data) 91 | { 92 | return; 93 | } 94 | 95 | #endif 96 | -------------------------------------------------------------------------------- /amcl/src/include/portable_utils.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PORTABLE_UTILS_H 2 | #define PORTABLE_UTILS_H 3 | 4 | #include 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | #ifndef HAVE_DRAND48 11 | // Some system (e.g., Windows) doesn't come with drand48(), srand48(). 12 | // Use rand, and srand for such system. 13 | static double drand48(void) 14 | { 15 | return ((double)rand())/RAND_MAX; 16 | } 17 | 18 | static void srand48(long int seedval) 19 | { 20 | srand(seedval); 21 | } 22 | #endif 23 | 24 | #ifdef __cplusplus 25 | } 26 | #endif 27 | 28 | #endif -------------------------------------------------------------------------------- /amcl/test/basic_localization_stage.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 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 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 51 | 52 | -------------------------------------------------------------------------------- /amcl/test/global_localization_stage.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 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 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 48 | 49 | -------------------------------------------------------------------------------- /amcl/test/rosie_multilaser.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 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 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 51 | 52 | -------------------------------------------------------------------------------- /amcl/test/set_initial_pose.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 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 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 51 | 52 | 54 | 55 | -------------------------------------------------------------------------------- /amcl/test/set_initial_pose_delayed.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 9 | 10 | 11 | 14 | 15 | 16 | 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 | 55 | 56 | 58 | 59 | -------------------------------------------------------------------------------- /amcl/test/set_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | 5 | import math 6 | import PyKDL 7 | from geometry_msgs.msg import PoseWithCovarianceStamped 8 | 9 | 10 | class PoseSetter(rospy.SubscribeListener): 11 | def __init__(self, pose, stamp, publish_time): 12 | self.pose = pose 13 | self.stamp = stamp 14 | self.publish_time = publish_time 15 | 16 | def peer_subscribe(self, topic_name, topic_publish, peer_publish): 17 | p = PoseWithCovarianceStamped() 18 | p.header.frame_id = "map" 19 | p.header.stamp = self.stamp 20 | p.pose.pose.position.x = self.pose[0] 21 | p.pose.pose.position.y = self.pose[1] 22 | (p.pose.pose.orientation.x, 23 | p.pose.pose.orientation.y, 24 | p.pose.pose.orientation.z, 25 | p.pose.pose.orientation.w) = PyKDL.Rotation.RPY(0, 0, self.pose[2]).GetQuaternion() 26 | p.pose.covariance[6*0+0] = 0.5 * 0.5 27 | p.pose.covariance[6*1+1] = 0.5 * 0.5 28 | p.pose.covariance[6*3+3] = math.pi/12.0 * math.pi/12.0 29 | # wait for the desired publish time 30 | while rospy.get_rostime() < self.publish_time: 31 | rospy.sleep(0.01) 32 | peer_publish(p) 33 | 34 | 35 | if __name__ == '__main__': 36 | pose = list(map(float, rospy.myargv()[1:4])) 37 | t_stamp = rospy.Time() 38 | t_publish = rospy.Time() 39 | if len(rospy.myargv()) > 4: 40 | t_stamp = rospy.Time.from_sec(float(rospy.myargv()[4])) 41 | if len(rospy.myargv()) > 5: 42 | t_publish = rospy.Time.from_sec(float(rospy.myargv()[5])) 43 | rospy.init_node('pose_setter', anonymous=True) 44 | rospy.loginfo("Going to publish pose {} with stamp {} at {}".format(pose, t_stamp.to_sec(), t_publish.to_sec())) 45 | pub = rospy.Publisher("initialpose", PoseWithCovarianceStamped, PoseSetter(pose, stamp=t_stamp, publish_time=t_publish), queue_size=1) 46 | rospy.spin() 47 | -------------------------------------------------------------------------------- /amcl/test/small_loop_crazy_driving_prg.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 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 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 51 | 52 | -------------------------------------------------------------------------------- /amcl/test/small_loop_crazy_driving_prg_corrected.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 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 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 51 | 52 | -------------------------------------------------------------------------------- /amcl/test/small_loop_prf.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 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 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 48 | 49 | -------------------------------------------------------------------------------- /amcl/test/texas_greenroom_loop.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 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 | 34 | 35 | 36 | 37 | 38 | 39 | 41 | 42 | -------------------------------------------------------------------------------- /amcl/test/texas_greenroom_loop_corrected.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 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 | 34 | 35 | 36 | 37 | 38 | 39 | 41 | 42 | -------------------------------------------------------------------------------- /amcl/test/texas_willow_hallway_loop.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 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 | 34 | 35 | 36 | 37 | 38 | 39 | 41 | 42 | -------------------------------------------------------------------------------- /amcl/test/texas_willow_hallway_loop_corrected.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 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 | 34 | 35 | 36 | 37 | 38 | 39 | 41 | 42 | -------------------------------------------------------------------------------- /base_local_planner/blp_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A implementation of a local planner using either a DWA or Trajectory Rollout approach based on configuration parameters. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /base_local_planner/cfg/LocalPlannerLimits.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Generic Local Planner configuration 3 | 4 | # from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | 7 | # if __name__ == "__main__": 8 | # gen = ParameterGenerator() 9 | # add_generic_localplanner_params(gen) 10 | # exit(gen.generate(PACKAGE, "base_local_planner", "LocalPlannerLimits")) 11 | -------------------------------------------------------------------------------- /base_local_planner/include/base_local_planner/map_cell.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | #ifndef TRAJECTORY_ROLLOUT_MAP_CELL_H_ 35 | #define TRAJECTORY_ROLLOUT_MAP_CELL_H_ 36 | 37 | #include 38 | 39 | namespace base_local_planner { 40 | /** 41 | * @class MapCell 42 | * @brief Stores path distance and goal distance information used for scoring trajectories 43 | */ 44 | class MapCell{ 45 | public: 46 | /** 47 | * @brief Default constructor 48 | */ 49 | MapCell(); 50 | 51 | /** 52 | * @brief Copy constructor 53 | * @param mc The MapCell to be copied 54 | */ 55 | MapCell(const MapCell& mc); 56 | 57 | unsigned int cx, cy; ///< @brief Cell index in the grid map 58 | 59 | double target_dist; ///< @brief Distance to planner's path 60 | 61 | bool target_mark; ///< @brief Marks for computing path/goal distances 62 | 63 | bool within_robot; ///< @brief Mark for cells within the robot footprint 64 | }; 65 | }; 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /base_local_planner/include/base_local_planner/planar_laser_scan.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Willow Garage nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | #ifndef TRAJECTORY_ROLLOUT_PLANAR_LASER_SCAN_H_ 38 | #define TRAJECTORY_ROLLOUT_PLANAR_LASER_SCAN_H_ 39 | 40 | #include 41 | #include 42 | 43 | namespace base_local_planner { 44 | /** 45 | * @class PlanarLaserScan 46 | * @brief Stores a scan from a planar laser that can be used to clear freespace 47 | */ 48 | class PlanarLaserScan { 49 | public: 50 | PlanarLaserScan() {} 51 | geometry_msgs::Point32 origin; 52 | sensor_msgs::PointCloud cloud; 53 | double angle_min, angle_max, angle_increment; 54 | }; 55 | }; 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /base_local_planner/include/base_local_planner/prefer_forward_cost_function.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: TKruse 36 | *********************************************************************/ 37 | 38 | #ifndef PREFER_FORWARD_COST_FUNCTION_H_ 39 | #define PREFER_FORWARD_COST_FUNCTION_H_ 40 | 41 | #include 42 | 43 | namespace base_local_planner { 44 | 45 | class PreferForwardCostFunction: public base_local_planner::TrajectoryCostFunction { 46 | public: 47 | 48 | PreferForwardCostFunction(double penalty) : penalty_(penalty) {} 49 | ~PreferForwardCostFunction() {} 50 | 51 | double scoreTrajectory(Trajectory &traj); 52 | 53 | bool prepare() {return true;}; 54 | 55 | void setPenalty(double penalty) { 56 | penalty_ = penalty; 57 | } 58 | 59 | private: 60 | double penalty_; 61 | }; 62 | 63 | } /* namespace base_local_planner */ 64 | #endif /* PREFER_FORWARD_COST_FUNCTION_H_ */ 65 | -------------------------------------------------------------------------------- /base_local_planner/include/base_local_planner/trajectory_cost_function.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: TKruse 36 | *********************************************************************/ 37 | 38 | #ifndef TRAJECTORYCOSTFUNCTION_H_ 39 | #define TRAJECTORYCOSTFUNCTION_H_ 40 | 41 | #include 42 | 43 | namespace base_local_planner { 44 | 45 | /** 46 | * @class TrajectoryCostFunction 47 | * @brief Provides an interface for critics of trajectories 48 | * During each sampling run, a batch of many trajectories will be scored using such a cost function. 49 | * The prepare method is called before each batch run, and then for each 50 | * trajectory of the sampling set, score_trajectory may be called. 51 | */ 52 | class TrajectoryCostFunction { 53 | public: 54 | 55 | /** 56 | * 57 | * 更新常用的变量 58 | * 毕竟纯虚函数，子类中必须重写该函数 59 | */ 60 | virtual bool prepare() = 0; 61 | 62 | /** 63 | * 返回一个局部轨迹的评分，也是纯虚函数 64 | */ 65 | virtual double scoreTrajectory(Trajectory &traj) = 0; 66 | 67 | double getScale() { 68 | return scale_; 69 | } 70 | 71 | void setScale(double scale) { 72 | scale_ = scale; 73 | } 74 | 75 | virtual ~TrajectoryCostFunction() {} 76 | 77 | protected: 78 | TrajectoryCostFunction(double scale = 1.0): scale_(scale) {} 79 | 80 | private: 81 | double scale_; 82 | }; 83 | 84 | } 85 | 86 | #endif /* TRAJECTORYCOSTFUNCTION_H_ */ 87 | -------------------------------------------------------------------------------- /base_local_planner/include/base_local_planner/trajectory_inc.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | #ifndef TRAJECTORY_INC_H_ 35 | #define TRAJECTORY_INC_H_ 36 | 37 | #include 38 | 39 | #ifndef DBL_MAX /* Max decimal value of a double */ 40 | #define DBL_MAX std::numeric_limits::max() // 1.7976931348623157e+308 41 | #endif 42 | 43 | #ifndef DBL_MIN //Min decimal value of a double 44 | #define DBL_MIN std::numeric_limits::min() // 2.2250738585072014e-308 45 | #endif 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /base_local_planner/include/base_local_planner/trajectory_sample_generator.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: TKruse 36 | *********************************************************************/ 37 | 38 | #ifndef TRAJECTORY_SAMPLE_GENERATOR_H_ 39 | #define TRAJECTORY_SAMPLE_GENERATOR_H_ 40 | 41 | #include 42 | 43 | namespace base_local_planner { 44 | 45 | /** 46 | * @class TrajectorySampleGenerator 47 | * @brief Provides an interface for navigation trajectory generators 48 | */ 49 | class TrajectorySampleGenerator { 50 | public: 51 | 52 | /** 53 | * 这个产生器是否可以生成多个路径 54 | */ 55 | virtual bool hasMoreTrajectories() = 0; 56 | 57 | /** 58 | * 这个产生器是否可以生产多个路径 59 | */ 60 | virtual bool nextTrajectory(Trajectory &traj) = 0; 61 | 62 | /** 63 | * @brief 接口的虚析构函数 64 | */ 65 | virtual ~TrajectorySampleGenerator() {} 66 | 67 | protected: 68 | TrajectorySampleGenerator() {} 69 | 70 | }; 71 | 72 | } // end namespace 73 | 74 | #endif /* TRAJECTORY_SAMPLE_GENERATOR_H_ */ 75 | -------------------------------------------------------------------------------- /base_local_planner/include/base_local_planner/trajectory_search.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: TKruse 36 | *********************************************************************/ 37 | 38 | #ifndef TRAJECTORY_SEARCH_H_ 39 | #define TRAJECTORY_SEARCH_H_ 40 | 41 | #include 42 | 43 | namespace base_local_planner { 44 | 45 | /** 46 | * @class TrajectorySearch 47 | * @brief 作为一个纯虚类，提供寻找局部轨迹的模块接口 48 | */ 49 | // ? 50 | class TrajectorySearch { 51 | public: 52 | /** 53 | * 在给定的空间里搜寻局部路径，在满足一些限制情况下，返回最优的一个 54 | * @param traj 存储计算结果的容器 55 | * @param all_explored 传递null或者搜集了所以路径的容器，用来debug(有一定惩罚) 56 | */ 57 | virtual bool findBestTrajectory(Trajectory& traj, std::vector* all_explored) = 0; 58 | 59 | virtual ~TrajectorySearch() {} 60 | 61 | protected: 62 | TrajectorySearch() {} 63 | 64 | }; 65 | 66 | 67 | } 68 | 69 | #endif /* TRAJECTORY_SEARCH_H_ */ 70 | -------------------------------------------------------------------------------- /base_local_planner/include/base_local_planner/twirling_cost_function.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2017, Open Source Robotics Foundation, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Morgan Quigley 36 | *********************************************************************/ 37 | 38 | #ifndef TWIRLING_COST_FUNCTION_H 39 | #define TWIRLING_COST_FUNCTION_H 40 | 41 | #include 42 | 43 | namespace base_local_planner { 44 | 45 | /** 46 | * This class provides a cost based on how much a robot "twirls" on its 47 | * way to the goal. With differential-drive robots, there isn't a choice, 48 | * but with holonomic or near-holonomic robots, sometimes a robot spins 49 | * more than you'd like on its way to a goal. This class provides a way 50 | * to assign a penalty purely to rotational velocities. 51 | */ 52 | class TwirlingCostFunction: public base_local_planner::TrajectoryCostFunction { 53 | public: 54 | 55 | TwirlingCostFunction() {} 56 | ~TwirlingCostFunction() {} 57 | 58 | double scoreTrajectory(Trajectory &traj); 59 | 60 | bool prepare() {return true;}; 61 | }; 62 | 63 | } /* namespace base_local_planner */ 64 | #endif /* TWIRLING_COST_FUNCTION_H_ */ 65 | -------------------------------------------------------------------------------- /base_local_planner/msg/Position2DInt.msg: -------------------------------------------------------------------------------- 1 | int64 x 2 | int64 y -------------------------------------------------------------------------------- /base_local_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | base_local_planner 5 | 1.17.1 6 | 7 | 8 | This package provides implementations of the Trajectory Rollout and Dynamic Window approaches to local robot navigation on a plane. Given a plan to follow and a costmap, the controller produces velocity commands to send to a mobile base. This package supports both holonomic and non-holonomic robots, any robot footprint that can be represented as a convex polygon or circle, and exposes its configuration as ROS parameters that can be set in a launch file. This package's ROS wrapper adheres to the BaseLocalPlanner interface specified in the nav_core package. 9 | 10 | 11 | Eitan Marder-Eppstein 12 | Eric Perko 13 | contradict@gmail.com 14 | Michael Ferguson 15 | David V. Lu!! 16 | Aaron Hoy 17 | BSD 18 | http://wiki.ros.org/base_local_planner 19 | 20 | catkin 21 | 22 | cmake_modules 23 | message_generation 24 | tf2_geometry_msgs 25 | 26 | angles 27 | costmap_2d 28 | dynamic_reconfigure 29 | eigen 30 | geometry_msgs 31 | nav_core 32 | nav_msgs 33 | pluginlib 34 | sensor_msgs 35 | std_msgs 36 | rosconsole 37 | roscpp 38 | rospy 39 | tf2 40 | tf2_ros 41 | visualization_msgs 42 | voxel_grid 43 | 44 | message_runtime 45 | 46 | rosunit 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /base_local_planner/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from distutils.core import setup 3 | from catkin_pkg.python_setup import generate_distutils_setup 4 | 5 | d = generate_distutils_setup( 6 | packages = ['local_planner_limits'], 7 | package_dir = {'': 'src'}, 8 | ) 9 | 10 | setup(**d) 11 | -------------------------------------------------------------------------------- /base_local_planner/src/local_planner_limits/.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | -------------------------------------------------------------------------------- /base_local_planner/src/local_planner_limits/__init__.py: -------------------------------------------------------------------------------- 1 | # Generic set of parameters to use with base local planners 2 | # To use: 3 | # 4 | # from local_planner_limits import add_generic_localplanner_params 5 | # gen = ParameterGenerator() 6 | # add_generic_localplanner_params(gen) 7 | # ... 8 | # 9 | # Using these standard parameters instead of your own allows easier switching of local planners 10 | 11 | # need this only for dataype declarations 12 | from dynamic_reconfigure.parameter_generator_catkin import double_t, bool_t 13 | 14 | 15 | def add_generic_localplanner_params(gen): 16 | # velocities 17 | gen.add("max_vel_trans", double_t, 0, "The absolute value of the maximum translational velocity for the robot in m/s", 0.55, 0) 18 | gen.add("min_vel_trans", double_t, 0, "The absolute value of the minimum translational velocity for the robot in m/s", 0.1, 0) 19 | 20 | gen.add("max_vel_x", double_t, 0, "The maximum x velocity for the robot in m/s", 0.55) 21 | gen.add("min_vel_x", double_t, 0, "The minimum x velocity for the robot in m/s", 0.0) 22 | 23 | gen.add("max_vel_y", double_t, 0, "The maximum y velocity for the robot in m/s", 0.1) 24 | gen.add("min_vel_y", double_t, 0, "The minimum y velocity for the robot in m/s", -0.1) 25 | 26 | gen.add("max_vel_theta", double_t, 0, "The absolute value of the maximum rotational velocity for the robot in rad/s", 1.0, 0) 27 | gen.add("min_vel_theta", double_t, 0, "The absolute value of the minimum rotational velocity for the robot in rad/s", 0.4, 0) 28 | 29 | # acceleration 30 | gen.add("acc_lim_x", double_t, 0, "The acceleration limit of the robot in the x direction", 2.5, 0, 20.0) 31 | gen.add("acc_lim_y", double_t, 0, "The acceleration limit of the robot in the y direction", 2.5, 0, 20.0) 32 | gen.add("acc_lim_theta", double_t, 0, "The acceleration limit of the robot in the theta direction", 3.2, 0, 20.0) 33 | gen.add("acc_lim_trans", double_t, 0, "The absolute value of the maximum translational acceleration for the robot in m/s^2", 0.1, 0) 34 | 35 | gen.add("prune_plan", bool_t, 0, "Start following closest point of global plan, not first point (if different).", False) 36 | 37 | gen.add("xy_goal_tolerance", double_t, 0, "Within what maximum distance we consider the robot to be in goal", 0.1) 38 | gen.add("yaw_goal_tolerance", double_t, 0, "Within what maximum angle difference we consider the robot to face goal direction", 0.1) 39 | 40 | gen.add("trans_stopped_vel", double_t, 0, "Below what maximum velocity we consider the robot to be stopped in translation", 0.1) 41 | gen.add("theta_stopped_vel", double_t, 0, "Below what maximum rotation velocity we consider the robot to be stopped in rotation", 0.1) 42 | -------------------------------------------------------------------------------- /base_local_planner/src/map_cell.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | 37 | namespace base_local_planner{ 38 | 39 | MapCell::MapCell() 40 | : cx(0), cy(0), 41 | target_dist(DBL_MAX), 42 | target_mark(false), 43 | within_robot(false) 44 | {} 45 | 46 | MapCell::MapCell(const MapCell& mc) 47 | : cx(mc.cx), cy(mc.cy), 48 | target_dist(mc.target_dist), 49 | target_mark(mc.target_mark), 50 | within_robot(mc.within_robot) 51 | {} 52 | }; 53 | -------------------------------------------------------------------------------- /base_local_planner/src/prefer_forward_cost_function.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * prefer_forward_cost_function.cpp 3 | * 4 | * Created on: Apr 4, 2012 5 | * Author: tkruse 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | namespace base_local_planner { 13 | 14 | 15 | double PreferForwardCostFunction::scoreTrajectory(Trajectory &traj) { 16 | // 如果机器人没有看向后面的传感器，强烈不建议后退 17 | if (traj.xv_ < 0.0) { 18 | return penalty_; 19 | } 20 | // 惩罚线速度和角速度都很小的运动 21 | if (traj.xv_ < 0.1 && fabs(traj.thetav_) < 0.2) { 22 | return penalty_; 23 | } 24 | // 旋转的越多，前进的越少 25 | return fabs(traj.thetav_) * 10; 26 | } 27 | 28 | } /* namespace base_local_planner */ 29 | -------------------------------------------------------------------------------- /base_local_planner/src/twirling_cost_function.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * twirling_cost_function.cpp 3 | * 4 | * Created on: Apr 20, 2016 5 | * Author: Morgan Quigley 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | namespace base_local_planner { 13 | 14 | double TwirlingCostFunction::scoreTrajectory(Trajectory &traj) { 15 | return fabs(traj.thetav_); // add cost for making the robot spin 16 | } 17 | 18 | } /* namespace base_local_planner */ 19 | -------------------------------------------------------------------------------- /base_local_planner/test/gtest_main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * gtest_main.cpp 3 | * 4 | * Created on: Apr 6, 2012 5 | * Author: tkruse 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | int main(int argc, char **argv) { 13 | std::cout << "Running main() from gtest_main.cc\n"; 14 | 15 | testing::InitGoogleTest(&argc, argv); 16 | return RUN_ALL_TESTS(); 17 | } 18 | 19 | -------------------------------------------------------------------------------- /base_local_planner/test/trajectory_generator_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * footprint_helper_test.cpp 3 | * 4 | * Created on: May 2, 2012 5 | * Author: tkruse 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | namespace base_local_planner { 15 | 16 | class TrajectoryGeneratorTest : public testing::Test { 17 | public: 18 | SimpleTrajectoryGenerator tg; 19 | 20 | TrajectoryGeneratorTest() { 21 | } 22 | 23 | virtual void TestBody(){} 24 | }; 25 | 26 | } 27 | -------------------------------------------------------------------------------- /base_local_planner/test/wavefront_map_accessor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * wavefront_map_accessor.h 3 | * 4 | * Created on: May 2, 2012 5 | * Author: tkruse 6 | */ 7 | 8 | #ifndef WAVEFRONT_MAP_ACCESSOR_H_ 9 | #define WAVEFRONT_MAP_ACCESSOR_H_ 10 | #include 11 | namespace base_local_planner { 12 | 13 | /** 14 | * Map_grids rely on costmaps to identify obstacles. We need a costmap that we can easily manipulate for unit tests. 15 | * This class has a grid map where we can set grid cell state, and a synchronize method to make the costmap match. 16 | */ 17 | class WavefrontMapAccessor : public costmap_2d::Costmap2D { 18 | public: 19 | WavefrontMapAccessor(MapGrid* map, double outer_radius) 20 | : costmap_2d::Costmap2D(map->size_x_, map->size_y_, 1, 0, 0), 21 | map_(map), outer_radius_(outer_radius) { 22 | synchronize(); 23 | } 24 | 25 | virtual ~WavefrontMapAccessor(){}; 26 | 27 | void synchronize(){ 28 | // Write Cost Data from the map 29 | for(unsigned int x = 0; x < size_x_; x++){ 30 | for (unsigned int y = 0; y < size_y_; y++){ 31 | unsigned int ind = x + (y * size_x_); 32 | if(map_->operator ()(x, y).target_dist == 1) { 33 | costmap_[ind] = costmap_2d::LETHAL_OBSTACLE; 34 | } else { 35 | costmap_[ind] = 0; 36 | } 37 | } 38 | } 39 | } 40 | 41 | private: 42 | MapGrid* map_; 43 | double outer_radius_; 44 | }; 45 | 46 | } 47 | 48 | 49 | 50 | #endif /* WAVEFRONT_MAP_ACCESSOR_H_ */ 51 | -------------------------------------------------------------------------------- /carrot_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(carrot_planner) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | angles 7 | base_local_planner 8 | costmap_2d 9 | nav_core 10 | pluginlib 11 | roscpp 12 | tf2 13 | tf2_geometry_msgs 14 | tf2_ros 15 | ) 16 | 17 | include_directories( 18 | include 19 | ${catkin_INCLUDE_DIRS} 20 | ) 21 | add_definitions(${EIGEN3_DEFINITIONS}) 22 | 23 | catkin_package( 24 | INCLUDE_DIRS include 25 | LIBRARIES carrot_planner 26 | CATKIN_DEPENDS 27 | angles 28 | base_local_planner 29 | costmap_2d 30 | nav_core 31 | pluginlib 32 | roscpp 33 | tf2 34 | tf2_ros 35 | ) 36 | 37 | add_library(carrot_planner src/carrot_planner.cpp) 38 | add_dependencies(carrot_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 39 | target_link_libraries(carrot_planner 40 | ${catkin_LIBRARIES} 41 | ) 42 | 43 | install(TARGETS carrot_planner 44 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 45 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 46 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 47 | ) 48 | 49 | install(DIRECTORY include/${PROJECT_NAME}/ 50 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 51 | PATTERN ".svn" EXCLUDE 52 | ) 53 | 54 | install(FILES bgp_plugin.xml 55 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 56 | ) 57 | 58 | 59 | -------------------------------------------------------------------------------- /carrot_planner/bgp_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A simple planner that seeks to place a legal carrot in-front of the robot 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /carrot_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | carrot_planner 5 | 1.17.1 6 | 7 | 8 | This planner attempts to find a legal place to put a carrot for the robot to follow. It does this by moving back along the vector between the robot and the goal point. 9 | 10 | 11 | Eitan Marder-Eppstein 12 | Sachin Chitta 13 | contradict@gmail.com 14 | Aaron Hoy 15 | David V. Lu!! 16 | Michael Ferguson 17 | BSD 18 | http://wiki.ros.org/carrot_planner 19 | 20 | catkin 21 | 22 | tf2_geometry_msgs 23 | 24 | angles 25 | base_local_planner 26 | costmap_2d 27 | eigen 28 | nav_core 29 | pluginlib 30 | roscpp 31 | tf2 32 | tf2_ros 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /clear_costmap_recovery/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(clear_costmap_recovery) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | cmake_modules 7 | costmap_2d 8 | nav_core 9 | pluginlib 10 | roscpp 11 | tf2_ros 12 | ) 13 | 14 | find_package(Eigen3 REQUIRED) 15 | remove_definitions(-DDISABLE_LIBUSB-1.0) 16 | include_directories( 17 | include 18 | ${catkin_INCLUDE_DIRS} 19 | ${EIGEN3_INCLUDE_DIRS} 20 | ) 21 | add_definitions(${EIGEN3_DEFINITIONS}) 22 | 23 | catkin_package( 24 | INCLUDE_DIRS include 25 | LIBRARIES clear_costmap_recovery 26 | CATKIN_DEPENDS 27 | costmap_2d 28 | nav_core 29 | pluginlib 30 | roscpp 31 | tf2_ros 32 | ) 33 | 34 | add_library(clear_costmap_recovery src/clear_costmap_recovery.cpp) 35 | add_dependencies(clear_costmap_recovery ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 36 | target_link_libraries(clear_costmap_recovery ${catkin_LIBRARIES}) 37 | 38 | 39 | ## Configure Tests 40 | if(CATKIN_ENABLE_TESTING) 41 | # Find package test dependencies 42 | find_package(rostest REQUIRED) 43 | 44 | # Add the test folder to the include directories 45 | include_directories(test) 46 | 47 | # Create targets for test executables 48 | add_rostest_gtest(clear_tester test/clear_tests.launch test/clear_tester.cpp) 49 | target_link_libraries(clear_tester clear_costmap_recovery ${GTEST_LIBRARIES}) 50 | endif() 51 | 52 | 53 | install(TARGETS clear_costmap_recovery 54 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 55 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 56 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 57 | ) 58 | 59 | install(FILES ccr_plugin.xml 60 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 61 | ) 62 | 63 | install(DIRECTORY include/${PROJECT_NAME}/ 64 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 65 | PATTERN ".svn" EXCLUDE 66 | ) 67 | -------------------------------------------------------------------------------- /clear_costmap_recovery/ccr_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A recovery behavior that reverts the costmap to the static map outside of a user specified window. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /clear_costmap_recovery/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | clear_costmap_recovery 5 | 1.17.1 6 | 7 | 8 | This package provides a recovery behavior for the navigation stack that attempts to clear space by reverting the costmaps used by the navigation stack to the static map outside of a given area. 9 | 10 | 11 | Eitan Marder-Eppstein 12 | contradict@gmail.com 13 | David V. Lu!! 14 | Michael Ferguson 15 | Aaron Hoy 16 | BSD 17 | http://wiki.ros.org/clear_costmap_recovery 18 | 19 | catkin 20 | 21 | cmake_modules 22 | 23 | costmap_2d 24 | eigen 25 | nav_core 26 | pluginlib 27 | roscpp 28 | tf2_ros 29 | 30 | rostest 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /clear_costmap_recovery/test/clear_tests.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /clear_costmap_recovery/test/params.yaml: -------------------------------------------------------------------------------- 1 | base: 2 | global: 3 | plugins: 4 | - {name: static_map, type: "costmap_2d::StaticLayer"} 5 | - {name: obstacles, type: "costmap_2d::ObstacleLayer"} 6 | - {name: inflation, type: "costmap_2d::InflationLayer"} 7 | obstacles: 8 | track_unknown_space: true 9 | local: 10 | plugins: [] 11 | robot_radius: .5 12 | clear: 13 | clearing_distance: 7.0 14 | -------------------------------------------------------------------------------- /costmap_2d/cfg/Costmap2D.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, str_t 4 | 5 | gen = ParameterGenerator() 6 | 7 | gen.add("transform_tolerance", double_t, 0, "Specifies the delay in transform (tf) data that is tolerable in seconds.", 0.3, 0, 10) 8 | gen.add("update_frequency", double_t, 0, "The frequency in Hz for the map to be updated.", 5, 0, 100) 9 | gen.add("publish_frequency", double_t, 0, "The frequency in Hz for the map to publish display information.", 0, 0, 100) 10 | 11 | #map params 12 | gen.add("width", int_t, 0, "The width of the map in meters.", 10, 0) 13 | gen.add("height", int_t, 0, "The height of the map in meters.", 10, 0) 14 | gen.add("resolution", double_t, 0, "The resolution of the map in meters/cell.", 0.05, 0, 50) 15 | gen.add("origin_x", double_t, 0, "The x origin of the map in the global frame in meters.", 0) 16 | gen.add("origin_y", double_t, 0, "The y origin of the map in the global frame in meters.", 0) 17 | 18 | # robot footprint shape 19 | gen.add("footprint", str_t, 0, "The footprint of the robot specified in the robot_base_frame coordinate frame as a list in the format: [ [x1, y1], [x2, y2], ...., [xn, yn] ].", "[]") 20 | gen.add("robot_radius", double_t, 0, 'The radius of the robot in meters, this parameter should only be set for circular robots, all others should use the footprint parameter described above.', 0.46, 0, 10) 21 | gen.add("footprint_padding", double_t, 0, "How much to pad (increase the size of) the footprint, in meters.", 0.01) 22 | 23 | exit(gen.generate("costmap_2d", "costmap_2d", "Costmap2D")) 24 | -------------------------------------------------------------------------------- /costmap_2d/cfg/GenericPlugin.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t 4 | 5 | gen = ParameterGenerator() 6 | gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) 7 | exit(gen.generate("costmap_2d", "costmap_2d", "GenericPlugin")) 8 | -------------------------------------------------------------------------------- /costmap_2d/cfg/InflationPlugin.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t 4 | 5 | gen = ParameterGenerator() 6 | 7 | gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) 8 | gen.add("cost_scaling_factor", double_t, 0, "A scaling factor to apply to cost values during inflation.", 10, 0, 100) 9 | gen.add("inflation_radius", double_t, 0, "The radius in meters to which the map inflates obstacle cost values.", 0.55, 0, 50) 10 | gen.add("inflate_unknown", bool_t, 0, "Whether to inflate unknown cells.", False) 11 | 12 | exit(gen.generate("costmap_2d", "costmap_2d", "InflationPlugin")) 13 | -------------------------------------------------------------------------------- /costmap_2d/cfg/ObstaclePlugin.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t 4 | 5 | gen = ParameterGenerator() 6 | 7 | gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) 8 | gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True) 9 | gen.add("max_obstacle_height", double_t, 0, "The maximum height of any obstacle to be inserted into the costmap in meters.", 2, 0, 50) 10 | 11 | combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), 12 | gen.const("Maximum", int_t, 1, "Take the maximum of the values"), 13 | gen.const("Nothing", int_t, 99, "Do nothing")], 14 | "Method for combining layers enum") 15 | gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, edit_method=combo_enum) 16 | 17 | 18 | #gen.add("max_obstacle_range", double_t, 0, "The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 2.5, 0, 50) 19 | #gen.add("raytrace_range", double_t, 0, "The default range in meters at which to raytrace out obstacles from the map using sensor data.", 3, 0, 50) 20 | exit(gen.generate("costmap_2d", "costmap_2d", "ObstaclePlugin")) 21 | -------------------------------------------------------------------------------- /costmap_2d/cfg/VoxelPlugin.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t 4 | 5 | gen = ParameterGenerator() 6 | 7 | gen.add("enabled", bool_t, 0, "Whether to use this plugin or not", True) 8 | gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True) 9 | gen.add("max_obstacle_height", double_t, 0, "Max Obstacle Height", 2.0, 0, 50) 10 | gen.add("origin_z", double_t, 0, "The z origin of the map in meters.", 0, 0) 11 | gen.add("z_resolution", double_t, 0, "The z resolution of the map in meters/cell.", 0.2, 0, 50) 12 | gen.add("z_voxels", int_t, 0, "The number of voxels to in each vertical column.", 10, 0, 16) 13 | gen.add("unknown_threshold", int_t, 0, 'The number of unknown cells allowed in a column considered to be known', 15, 0, 16) 14 | gen.add("mark_threshold", int_t, 0, 'The maximum number of marked cells allowed in a column considered to be free', 0, 0, 16) 15 | 16 | combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), 17 | gen.const("Maximum", int_t, 1, "Take the maximum of the values"), 18 | gen.const("Nothing", int_t, 99, "Do nothing")], 19 | "Method for combining layers enum") 20 | gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, 0, 2, edit_method=combo_enum) 21 | 22 | exit(gen.generate("costmap_2d", "costmap_2d", "VoxelPlugin")) 23 | -------------------------------------------------------------------------------- /costmap_2d/costmap_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Inflates obstacles to speed collision checking and to make robot prefer to stay away from obstacles. 5 | 6 | 7 | Listens to laser scan and point cloud messages and marks and clears grid cells. 8 | 9 | 10 | Listens to OccupancyGrid messages and copies them in, like from map_server. 11 | 12 | 13 | Similar to obstacle costmap, but uses 3D voxel grid to store data. 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /costmap_2d/include/costmap_2d/array_parser.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 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 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * author: Dave Hershberger 30 | */ 31 | #ifndef COSTMAP_2D_ARRAY_PARSER_H 32 | #define COSTMAP_2D_ARRAY_PARSER_H 33 | 34 | #include 35 | #include 36 | 37 | namespace costmap_2d 38 | { 39 | 40 | /** @brief Parse a vector of vectors of floats from a string. 41 | * @param error_return If no error, error_return is set to "". If 42 | * error, error_return will describe the error. 43 | * Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...] 44 | * 45 | * On error, error_return is set and the return value could be 46 | * anything, like part of a successful parse. */ 47 | std::vector > parseVVF(const std::string& input, std::string& error_return); 48 | 49 | } // end namespace costmap_2d 50 | 51 | #endif // COSTMAP_2D_ARRAY_PARSER_H 52 | -------------------------------------------------------------------------------- /costmap_2d/include/costmap_2d/cost_values.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, 2013, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | #ifndef COSTMAP_2D_COST_VALUES_H_ 38 | #define COSTMAP_2D_COST_VALUES_H_ 39 | /** Provides a mapping for often used cost values */ 40 | namespace costmap_2d 41 | { 42 | static const unsigned char NO_INFORMATION = 255; 43 | static const unsigned char LETHAL_OBSTACLE = 254; 44 | static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; 45 | static const unsigned char FREE_SPACE = 0; 46 | } 47 | #endif // COSTMAP_2D_COST_VALUES_H_ 48 | -------------------------------------------------------------------------------- /costmap_2d/include/costmap_2d/costmap_math.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, 2013, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | * David V. Lu!! 37 | *********************************************************************/ 38 | #ifndef COSTMAP_2D_COSTMAP_MATH_H_ 39 | #define COSTMAP_2D_COSTMAP_MATH_H_ 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | /** @brief Return -1 if x < 0, +1 otherwise. */ 47 | inline double sign(double x) 48 | { 49 | return x < 0.0 ? -1.0 : 1.0; 50 | } 51 | 52 | /** @brief Same as sign(x) but returns 0 if x is 0. */ 53 | inline double sign0(double x) 54 | { 55 | return x < 0.0 ? -1.0 : (x > 0.0 ? 1.0 : 0.0); 56 | } 57 | 58 | inline double distance(double x0, double y0, double x1, double y1) 59 | { 60 | return hypot(x1 - x0, y1 - y0); 61 | } 62 | 63 | double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1); 64 | 65 | bool intersects(std::vector& polygon, float testx, float testy); 66 | 67 | bool intersects(std::vector& polygon1, std::vector& polygon2); 68 | 69 | #endif // COSTMAP_2D_COSTMAP_MATH_H_ 70 | -------------------------------------------------------------------------------- /costmap_2d/launch/example.launch: -------------------------------------------------------------------------------- 1 | 2 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /costmap_2d/launch/example_params.yaml: -------------------------------------------------------------------------------- 1 | global_frame: map 2 | robot_base_frame: base_link 3 | update_frequency: 5.0 4 | publish_frequency: 1.0 5 | 6 | #set if you want the voxel map published 7 | publish_voxel_map: true 8 | 9 | #set to true if you want to initialize the costmap from a static map 10 | static_map: false 11 | 12 | #begin - COMMENT these lines if you set static_map to true 13 | rolling_window: true 14 | width: 6.0 15 | height: 6.0 16 | resolution: 0.025 17 | #end - COMMENT these lines if you set static_map to true 18 | 19 | #START VOXEL STUFF 20 | map_type: voxel 21 | origin_z: 0.0 22 | z_resolution: 0.2 23 | z_voxels: 10 24 | unknown_threshold: 10 25 | mark_threshold: 0 26 | #END VOXEL STUFF 27 | 28 | transform_tolerance: 0.3 29 | obstacle_range: 2.5 30 | max_obstacle_height: 2.0 31 | raytrace_range: 3.0 32 | footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] 33 | #robot_radius: 0.46 34 | footprint_padding: 0.01 35 | inflation_radius: 0.55 36 | cost_scaling_factor: 10.0 37 | lethal_cost_threshold: 100 38 | observation_sources: base_scan 39 | base_scan: {data_type: LaserScan, expected_update_rate: 0.4, 40 | observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08} 41 | -------------------------------------------------------------------------------- /costmap_2d/msg/VoxelGrid.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint32[] data 3 | geometry_msgs/Point32 origin 4 | geometry_msgs/Vector3 resolutions 5 | uint32 size_x 6 | uint32 size_y 7 | uint32 size_z 8 | 9 | -------------------------------------------------------------------------------- /costmap_2d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | costmap_2d 5 | 1.17.1 6 | 7 | This package provides an implementation of a 2D costmap that takes in sensor 8 | data from the world, builds a 2D or 3D occupancy grid of the data (depending 9 | on whether a voxel based implementation is used), and inflates costs in a 10 | 2D costmap based on the occupancy grid and a user specified inflation radius. 11 | This package also provides support for map_server based initialization of a 12 | costmap, rolling window based costmaps, and parameter based subscription to 13 | and configuration of sensor topics. 14 | 15 | Eitan Marder-Eppstein 16 | David V. Lu!! 17 | Dave Hershberger 18 | contradict@gmail.com 19 | David V. Lu!! 20 | Michael Ferguson 21 | Aaron Hoy 22 | BSD 23 | http://wiki.ros.org/costmap_2d 24 | 25 | catkin 26 | 27 | cmake_modules 28 | message_generation 29 | tf2_geometry_msgs 30 | tf2_sensor_msgs 31 | 32 | dynamic_reconfigure 33 | eigen 34 | geometry_msgs 35 | laser_geometry 36 | map_msgs 37 | message_filters 38 | nav_msgs 39 | pluginlib 40 | roscpp 41 | rostest 42 | sensor_msgs 43 | std_msgs 44 | tf2 45 | tf2_ros 46 | visualization_msgs 47 | voxel_grid 48 | 49 | message_runtime 50 | rosconsole 51 | map_server 52 | rosbag 53 | rostest 54 | rosunit 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /costmap_2d/src/costmap_2d_node.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, 2013, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | * David V. Lu!! 37 | *********************************************************************/ 38 | #include 39 | #include 40 | #include 41 | 42 | int main(int argc, char** argv) 43 | { 44 | ros::init(argc, argv, "costmap_node"); 45 | tf2_ros::Buffer buffer(ros::Duration(10)); 46 | tf2_ros::TransformListener tf(buffer); 47 | costmap_2d::Costmap2DROS lcr("costmap", buffer); 48 | 49 | ros::spin(); 50 | 51 | return (0); 52 | } 53 | -------------------------------------------------------------------------------- /costmap_2d/src/layer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 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 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "costmap_2d/layer.h" 31 | 32 | namespace costmap_2d 33 | { 34 | 35 | Layer::Layer() 36 | : layered_costmap_(NULL) 37 | , current_(false) 38 | , enabled_(false) 39 | , name_() 40 | , tf_(NULL) 41 | {} 42 | 43 | void Layer::initialize(LayeredCostmap* parent, std::string name, tf2_ros::Buffer *tf) 44 | { 45 | layered_costmap_ = parent; 46 | name_ = name; 47 | tf_ = tf; 48 | onInitialize(); 49 | } 50 | 51 | const std::vector& Layer::getFootprint() const 52 | { 53 | return layered_costmap_->getFootprint(); 54 | } 55 | 56 | } // end namespace costmap_2d 57 | -------------------------------------------------------------------------------- /costmap_2d/test/TenByTen.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/felderstehost/ros-navigation-noetic/fc1f166d744b5d61ee2815a2bb0d69b3a9b69428/costmap_2d/test/TenByTen.pgm -------------------------------------------------------------------------------- /costmap_2d/test/TenByTen.yaml: -------------------------------------------------------------------------------- 1 | image: TenByTen.pgm 2 | resolution: 1.0 3 | origin: [0,0,0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /costmap_2d/test/array_parser_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 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 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | 32 | #include "costmap_2d/array_parser.h" 33 | 34 | using namespace costmap_2d; 35 | 36 | TEST(array_parser, basic_operation) 37 | { 38 | std::string error; 39 | std::vector > vvf; 40 | vvf = parseVVF( "[[1, 2.2], [.3, -4e4]]", error ); 41 | EXPECT_EQ( 2, vvf.size() ); 42 | EXPECT_EQ( 2, vvf[0].size() ); 43 | EXPECT_EQ( 2, vvf[1].size() ); 44 | EXPECT_EQ( 1.0f, vvf[0][0] ); 45 | EXPECT_EQ( 2.2f, vvf[0][1] ); 46 | EXPECT_EQ( 0.3f, vvf[1][0] ); 47 | EXPECT_EQ( -40000.0f, vvf[1][1] ); 48 | EXPECT_EQ( "", error ); 49 | } 50 | 51 | TEST(array_parser, missing_open) 52 | { 53 | std::string error; 54 | std::vector > vvf; 55 | vvf = parseVVF( "[1, 2.2], [.3, -4e4]]", error ); 56 | EXPECT_FALSE( error == "" ); 57 | } 58 | 59 | TEST(array_parser, missing_close) 60 | { 61 | std::string error; 62 | std::vector > vvf; 63 | vvf = parseVVF( "[[1, 2.2], [.3, -4e4]", error ); 64 | EXPECT_FALSE( error == "" ); 65 | } 66 | 67 | TEST(array_parser, wrong_depth) 68 | { 69 | std::string error; 70 | std::vector > vvf; 71 | vvf = parseVVF( "[1, 2.2], [.3, -4e4]", error ); 72 | EXPECT_FALSE( error == "" ); 73 | } 74 | 75 | int main(int argc, char** argv) 76 | { 77 | testing::InitGoogleTest( &argc, argv ); 78 | return RUN_ALL_TESTS(); 79 | } 80 | 81 | -------------------------------------------------------------------------------- /costmap_2d/test/costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_frame: map 2 | robot_base_frame: base_link 3 | update_frequency: 5.0 4 | publish_frequency: 0.0 5 | static_map: true 6 | rolling_window: false 7 | 8 | #START VOXEL STUFF 9 | map_type: voxel 10 | origin_z: 0.0 11 | z_resolution: 0.2 12 | z_voxels: 10 13 | unknown_threshold: 10 14 | mark_threshold: 0 15 | #END VOXEL STUFF 16 | 17 | transform_tolerance: 0.3 18 | obstacle_range: 2.5 19 | max_obstacle_height: 2.0 20 | raytrace_range: 3.0 21 | footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] 22 | #robot_radius: 0.46 23 | footprint_padding: 0.01 24 | inflation_radius: 0.55 25 | cost_scaling_factor: 10.0 26 | lethal_cost_threshold: 100 27 | observation_sources: base_scan 28 | base_scan: {data_type: LaserScan, expected_update_rate: 0.4, 29 | observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08} 30 | -------------------------------------------------------------------------------- /costmap_2d/test/footprint_tests.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | footprint_padding: 0 15 | footprint: [[0.1, 0.1], [-0.1, 0.1], [-0.1, -0.1], [0.1, -0.1]] 16 | 17 | 18 | 19 | footprint_padding: 0 20 | footprint: [[0.1, 0.1], [-0.1, 0.1, 77.0], [-0.1, -0.1], [0.1, -0.1]] 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /costmap_2d/test/inflation_tests.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /costmap_2d/test/obstacle_tests.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /costmap_2d/test/simple_driving_test.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /costmap_2d/test/static_tests.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /dwa_local_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(dwa_local_planner) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | angles 7 | base_local_planner 8 | cmake_modules 9 | costmap_2d 10 | dynamic_reconfigure 11 | nav_core 12 | nav_msgs 13 | pluginlib 14 | sensor_msgs 15 | roscpp 16 | tf2 17 | tf2_geometry_msgs 18 | tf2_ros 19 | ) 20 | 21 | find_package(Eigen3 REQUIRED) 22 | remove_definitions(-DDISABLE_LIBUSB-1.0) 23 | include_directories( 24 | include 25 | ${catkin_INCLUDE_DIRS} 26 | ${EIGEN3_INCLUDE_DIRS} 27 | ) 28 | add_definitions(${EIGEN3_DEFINITIONS}) 29 | 30 | # dynamic reconfigure 31 | generate_dynamic_reconfigure_options( 32 | cfg/DWAPlanner.cfg 33 | ) 34 | 35 | catkin_package( 36 | INCLUDE_DIRS include 37 | LIBRARIES dwa_local_planner 38 | CATKIN_DEPENDS 39 | base_local_planner 40 | dynamic_reconfigure 41 | nav_msgs 42 | pluginlib 43 | sensor_msgs 44 | roscpp 45 | tf2 46 | tf2_ros 47 | ) 48 | 49 | add_library(dwa_local_planner src/dwa_planner.cpp src/dwa_planner_ros.cpp) 50 | add_dependencies(dwa_local_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 51 | target_link_libraries(dwa_local_planner ${catkin_LIBRARIES}) 52 | 53 | install(TARGETS dwa_local_planner 54 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 55 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 56 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 57 | ) 58 | 59 | install(FILES blp_plugin.xml 60 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 61 | ) 62 | 63 | install(DIRECTORY include/${PROJECT_NAME}/ 64 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 65 | PATTERN ".svn" EXCLUDE 66 | ) 67 | -------------------------------------------------------------------------------- /dwa_local_planner/blp_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A implementation of a local planner using either a DWA approach based on configuration parameters. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /dwa_local_planner/cfg/DWAPlanner.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # DWA Planner configuration 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, bool_t 5 | from local_planner_limits import add_generic_localplanner_params 6 | 7 | gen = ParameterGenerator() 8 | 9 | # This unusual line allows to reuse existing parameter definitions 10 | # that concern all localplanners 11 | add_generic_localplanner_params(gen) 12 | #添加参数 （name , type, level, description, default, min, max） 13 | gen.add("sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 1.7, 0) 14 | gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0) 15 | gen.add("angular_sim_granularity", double_t, 0, "The granularity with which to check for collisions for rotations in radians", 0.1, 0) 16 | 17 | gen.add("path_distance_bias", double_t, 0, "The weight for the path distance part of the cost function", 0.6, 0.0) 18 | gen.add("goal_distance_bias", double_t, 0, "The weight for the goal distance part of the cost function", 0.8, 0.0) 19 | gen.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.01, 0.0) 20 | gen.add("twirling_scale", double_t, 0, "The weight for penalizing any changes in robot heading", 0.0, 0.0) 21 | 22 | gen.add("stop_time_buffer", double_t, 0, "The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds", 0.2, 0) 23 | gen.add("oscillation_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.05, 0) 24 | gen.add("oscillation_reset_angle", double_t, 0, "The angle the robot must turn before oscillation flags are reset, in radians", 0.2, 0) 25 | 26 | gen.add("forward_point_distance", double_t, 0, "The distance from the center point of the robot to place an additional scoring point, in meters", 0.325) 27 | 28 | gen.add("scaling_speed", double_t, 0, "The absolute value of the velocity at which to start scaling the robot's footprint, in m/s", 0.25, 0) 29 | gen.add("max_scaling_factor", double_t, 0, "The maximum factor to scale the robot's footprint by", 0.2, 0) 30 | 31 | gen.add("vx_samples", int_t, 0, "The number of samples to use when exploring the x velocity space", 3, 1) 32 | gen.add("vy_samples", int_t, 0, "The number of samples to use when exploring the y velocity space", 10, 1) 33 | gen.add("vth_samples", int_t, 0, "The number of samples to use when exploring the theta velocity space", 20, 1) 34 | 35 | gen.add("use_dwa", bool_t, 0, "Use dynamic window approach to constrain sampling velocities to small window.", True) 36 | 37 | gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration.", False) 38 | 39 | exit(gen.generate("dwa_local_planner", "dwa_local_planner", "DWAPlanner")) 40 | -------------------------------------------------------------------------------- /dwa_local_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | dwa_local_planner 5 | 1.17.1 6 | 7 | 8 | This package provides an implementation of the Dynamic Window Approach to 9 | local robot navigation on a plane. Given a global plan to follow and a 10 | costmap, the local planner produces velocity commands to send to a mobile 11 | base. This package supports any robot who's footprint can be represented as 12 | a convex polygon or cicrle, and exposes its configuration as ROS parameters 13 | that can be set in a launch file. The parameters for this planner are also 14 | dynamically reconfigurable. This package's ROS wrapper adheres to the 15 | BaseLocalPlanner interface specified in the nav_core package. 16 | 17 | 18 | Eitan Marder-Eppstein 19 | contradict@gmail.com 20 | David V. Lu!! 21 | Michael Ferguson 22 | Aaron Hoy 23 | BSD 24 | http://wiki.ros.org/dwa_local_planner 25 | 26 | catkin 27 | 28 | angles 29 | cmake_modules 30 | 31 | base_local_planner 32 | costmap_2d 33 | dynamic_reconfigure 34 | eigen 35 | nav_core 36 | nav_msgs 37 | pluginlib 38 | sensor_msgs 39 | roscpp 40 | tf2 41 | tf2_geometry_msgs 42 | tf2_ros 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /fake_localization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(fake_localization) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | angles 7 | geometry_msgs 8 | message_filters 9 | nav_msgs 10 | rosconsole 11 | roscpp 12 | rospy 13 | tf2_geometry_msgs 14 | tf2_ros 15 | ) 16 | 17 | 18 | find_package(Boost REQUIRED) 19 | 20 | catkin_package( 21 | CATKIN_DEPENDS 22 | geometry_msgs 23 | nav_msgs 24 | roscpp 25 | rospy 26 | ) 27 | 28 | include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 29 | 30 | add_executable(fake_localization fake_localization.cpp) 31 | target_link_libraries(fake_localization 32 | ${catkin_LIBRARIES} 33 | ${Boost_LIBRARIES} 34 | ) 35 | add_dependencies(fake_localization ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 36 | 37 | install( 38 | PROGRAMS 39 | static_odom_broadcaster.py 40 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 41 | ) 42 | 43 | install( 44 | TARGETS 45 | fake_localization 46 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 47 | ) 48 | -------------------------------------------------------------------------------- /fake_localization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | fake_localization 5 | 1.17.1 6 | A ROS node that simply forwards odometry information. 7 | Ioan A. Sucan 8 | contradict@gmail.com 9 | David V. Lu!! 10 | Michael Ferguson 11 | Aaron Hoy 12 | BSD 13 | http://wiki.ros.org/fake_localization 14 | 15 | catkin 16 | 17 | angles 18 | tf2_geometry_msgs 19 | 20 | geometry_msgs 21 | message_filters 22 | nav_msgs 23 | rosconsole 24 | roscpp 25 | rospy 26 | tf2_ros 27 | 28 | 29 | -------------------------------------------------------------------------------- /fake_localization/static_odom_broadcaster.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # 4 | # Similar to static_transform_broadcaster, this node constantly publishes 5 | # static odometry information (Odometry msg and tf). This can be used 6 | # with fake_localization to evaluate planning algorithms without running 7 | # an actual robot with odometry or localization 8 | # 9 | # Author: Armin Hornung 10 | # License: BSD 11 | 12 | import rospy 13 | 14 | import tf 15 | from nav_msgs.msg import Odometry 16 | from geometry_msgs.msg import Pose, Quaternion, Point 17 | 18 | 19 | def publishOdom(): 20 | rospy.init_node('fake_odom') 21 | base_frame_id = rospy.get_param("~base_frame_id", "base_link") 22 | odom_frame_id = rospy.get_param("~odom_frame_id", "odom") 23 | publish_frequency = rospy.get_param("~publish_frequency", 10.0) 24 | pub = rospy.Publisher('odom', Odometry) 25 | tf_pub = tf.TransformBroadcaster() 26 | 27 | #TODO: static pose could be made configurable (cmd.line or parameters) 28 | quat = tf.transformations.quaternion_from_euler(0, 0, 0) 29 | 30 | odom = Odometry() 31 | odom.header.frame_id = odom_frame_id 32 | odom.pose.pose = Pose(Point(0, 0, 0), Quaternion(*quat)) 33 | 34 | rospy.loginfo("Publishing static odometry from \"%s\" to \"%s\"", odom_frame_id, base_frame_id) 35 | r = rospy.Rate(publish_frequency) 36 | while not rospy.is_shutdown(): 37 | odom.header.stamp = rospy.Time.now() 38 | pub.publish(odom) 39 | tf_pub.sendTransform((0, 0, 0), quat, 40 | odom.header.stamp, base_frame_id, odom_frame_id) 41 | r.sleep() 42 | 43 | if __name__ == '__main__': 44 | try: 45 | publishOdom() 46 | except rospy.ROSInterruptException: 47 | pass 48 | -------------------------------------------------------------------------------- /global_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(global_planner) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | angles 7 | costmap_2d 8 | dynamic_reconfigure 9 | geometry_msgs 10 | nav_core 11 | navfn 12 | nav_msgs 13 | pluginlib 14 | roscpp 15 | tf2_geometry_msgs 16 | tf2_ros 17 | ) 18 | 19 | generate_dynamic_reconfigure_options( 20 | cfg/GlobalPlanner.cfg 21 | ) 22 | 23 | catkin_package( 24 | INCLUDE_DIRS include 25 | LIBRARIES ${PROJECT_NAME} 26 | CATKIN_DEPENDS 27 | costmap_2d 28 | dynamic_reconfigure 29 | geometry_msgs 30 | nav_core 31 | navfn 32 | nav_msgs 33 | pluginlib 34 | roscpp 35 | tf2_ros 36 | ) 37 | 38 | include_directories( 39 | include 40 | ${catkin_INCLUDE_DIRS} 41 | ) 42 | 43 | add_library(${PROJECT_NAME} 44 | src/quadratic_calculator.cpp 45 | src/dijkstra.cpp 46 | src/astar.cpp 47 | src/grid_path.cpp 48 | src/gradient_path.cpp 49 | src/orientation_filter.cpp 50 | src/planner_core.cpp 51 | ) 52 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 53 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 54 | 55 | add_executable(planner 56 | src/plan_node.cpp 57 | ) 58 | add_dependencies(planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 59 | target_link_libraries(planner 60 | ${PROJECT_NAME} 61 | ${catkin_LIBRARIES} 62 | ) 63 | 64 | install(TARGETS ${PROJECT_NAME} 65 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 66 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 67 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) 68 | 69 | install(TARGETS planner 70 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 71 | 72 | install(DIRECTORY include/${PROJECT_NAME}/ 73 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 74 | PATTERN ".svn" EXCLUDE) 75 | 76 | install(FILES bgp_plugin.xml 77 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 78 | -------------------------------------------------------------------------------- /global_planner/bgp_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A implementation of a grid based planner using Dijkstras or A* 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /global_planner/cfg/GlobalPlanner.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "global_planner" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, int_t, double_t, bool_t 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("lethal_cost", int_t, 0, "Lethal Cost", 253, 1, 255) 9 | gen.add("neutral_cost", int_t, 0, "Neutral Cost", 50, 1, 255) 10 | gen.add("cost_factor", double_t, 0, "Factor to multiply each cost from costmap by", 3.0, 0.01, 5.0) 11 | gen.add("publish_potential", bool_t, 0, "Publish Potential Costmap", True) 12 | 13 | orientation_enum = gen.enum([ 14 | gen.const("None", int_t, 0, "No orientations added except goal orientation"), 15 | gen.const("Forward", int_t, 1, 16 | "Positive x axis points along path, except for the goal orientation"), 17 | gen.const("Interpolate", int_t, 2, "Orientations are a linear blend of start and goal pose"), 18 | gen.const("ForwardThenInterpolate", 19 | int_t, 3, "Forward orientation until last straightaway, then a linear blend until the goal pose"), 20 | gen.const("Backward", int_t, 4, 21 | "Negative x axis points along the path, except for the goal orientation"), 22 | gen.const("Leftward", int_t, 5, 23 | "Positive y axis points along the path, except for the goal orientation"), 24 | gen.const("Rightward", int_t, 6, 25 | "Negative y axis points along the path, except for the goal orientation"), 26 | ], "How to set the orientation of each point") 27 | 28 | gen.add("orientation_mode", int_t, 0, "How to set the orientation of each point", 1, 0, 6, 29 | edit_method=orientation_enum) 30 | gen.add("orientation_window_size", int_t, 0, "What window to use to determine the orientation based on the " 31 | "position derivative specified by the orientation mode", 1, 1, 255) 32 | 33 | exit(gen.generate(PACKAGE, "global_planner", "GlobalPlanner")) 34 | -------------------------------------------------------------------------------- /global_planner/include/global_planner/astar.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, 2013, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | * David V. Lu!! 37 | *********************************************************************/ 38 | #ifndef _ASTAR_H 39 | #define _ASTAR_H 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace global_planner { 47 | class Index { 48 | public: 49 | Index(int a, float b) { 50 | i = a; 51 | cost = b; 52 | } 53 | int i; 54 | float cost; 55 | }; 56 | 57 | struct greater1 { 58 | bool operator()(const Index& a, const Index& b) const { 59 | return a.cost > b.cost; 60 | } 61 | }; 62 | 63 | class AStarExpansion : public Expander { 64 | public: 65 | AStarExpansion(PotentialCalculator* p_calc, int nx, int ny); 66 | virtual ~AStarExpansion() {} 67 | bool calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y, int cycles, 68 | float* potential); 69 | private: 70 | void add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x, int end_y); 71 | std::vector queue_; 72 | }; 73 | 74 | } //end namespace global_planner 75 | #endif 76 | 77 | -------------------------------------------------------------------------------- /global_planner/include/global_planner/grid_path.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, 2013, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | * David V. Lu!! 37 | *********************************************************************/ 38 | #ifndef _GRID_PATH_H 39 | #define _GRID_PATH_H 40 | #include 41 | #include 42 | 43 | namespace global_planner { 44 | 45 | class GridPath : public Traceback { 46 | public: 47 | GridPath(PotentialCalculator* p_calc): Traceback(p_calc){} 48 | virtual ~GridPath() {} 49 | bool getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector >& path); 50 | }; 51 | 52 | } //end namespace global_planner 53 | #endif 54 | -------------------------------------------------------------------------------- /global_planner/include/global_planner/orientation_filter.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, David V. Lu!! 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of David V. Lu nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: David V. Lu!! 36 | *********************************************************************/ 37 | #ifndef GLOBAL_PLANNER_ORIENTATION_FILTER_H 38 | #define GLOBAL_PLANNER_ORIENTATION_FILTER_H 39 | #include 40 | 41 | namespace global_planner { 42 | 43 | enum OrientationMode { NONE, FORWARD, INTERPOLATE, FORWARDTHENINTERPOLATE, BACKWARD, LEFTWARD, RIGHTWARD }; 44 | 45 | class OrientationFilter { 46 | public: 47 | OrientationFilter() : omode_(NONE) {} 48 | 49 | 50 | virtual void processPath(const geometry_msgs::PoseStamped& start, 51 | std::vector& path); 52 | 53 | void setAngleBasedOnPositionDerivative(std::vector& path, int index); 54 | void interpolate(std::vector& path, 55 | int start_index, int end_index); 56 | 57 | void setMode(OrientationMode new_mode){ omode_ = new_mode; } 58 | void setMode(int new_mode){ setMode((OrientationMode) new_mode); } 59 | 60 | void setWindowSize(size_t window_size){ window_size_ = window_size; } 61 | protected: 62 | OrientationMode omode_; 63 | int window_size_; 64 | }; 65 | 66 | } //end namespace global_planner 67 | #endif 68 | -------------------------------------------------------------------------------- /global_planner/include/global_planner/quadratic_calculator.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, 2013, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | * David V. Lu!! 37 | *********************************************************************/ 38 | #ifndef _QUADRATIC_CALCULATOR_H 39 | #define _QUADRATIC_CALCULATOR_H 40 | #include 41 | #include 42 | 43 | namespace global_planner { 44 | 45 | class QuadraticCalculator : public PotentialCalculator { 46 | public: 47 | QuadraticCalculator(int nx, int ny): PotentialCalculator(nx,ny) {} 48 | 49 | float calculatePotential(float* potential, unsigned char cost, int n, float prev_potential); 50 | }; 51 | 52 | 53 | } //end namespace global_planner 54 | #endif 55 | -------------------------------------------------------------------------------- /global_planner/include/global_planner/traceback.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, 2013, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | * David V. Lu!! 37 | *********************************************************************/ 38 | #ifndef _TRACEBACK_H 39 | #define _TRACEBACK_H 40 | #include 41 | #include 42 | 43 | namespace global_planner { 44 | 45 | class Traceback { 46 | public: 47 | Traceback(PotentialCalculator* p_calc) : p_calc_(p_calc) {} 48 | virtual ~Traceback() {} 49 | virtual bool getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector >& path) = 0; 50 | virtual void setSize(int xs, int ys) { 51 | xs_ = xs; 52 | ys_ = ys; 53 | } 54 | inline int getIndex(int x, int y) { 55 | return x + y * xs_; 56 | } 57 | void setLethalCost(unsigned char lethal_cost) { 58 | lethal_cost_ = lethal_cost; 59 | } 60 | protected: 61 | int xs_, ys_; 62 | unsigned char lethal_cost_; 63 | PotentialCalculator* p_calc_; // 计算“一个点”的可行性 64 | }; 65 | 66 | } //end namespace global_planner 67 | #endif 68 | -------------------------------------------------------------------------------- /global_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | global_planner 5 | 1.17.1 6 | 7 | A path planner library and node. 8 | 9 | David V. Lu!! 10 | Michael Ferguson 11 | Aaron Hoy 12 | BSD 13 | 14 | David Lu!! 15 | http://wiki.ros.org/global_planner 16 | 17 | catkin 18 | 19 | angles 20 | tf2_geometry_msgs 21 | 22 | costmap_2d 23 | dynamic_reconfigure 24 | geometry_msgs 25 | nav_core 26 | nav_msgs 27 | navfn 28 | pluginlib 29 | roscpp 30 | tf2_ros 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /global_planner/src/quadratic_calculator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 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 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | 32 | namespace global_planner { 33 | float QuadraticCalculator::calculatePotential(float* potential, unsigned char cost, int n, float prev_potential) { 34 | // 获得邻点的potential 35 | float u, d, l, r; 36 | l = potential[n - 1]; 37 | r = potential[n + 1]; 38 | u = potential[n - nx_]; 39 | d = potential[n + nx_]; 40 | // ROS_INFO("[Update] c: %f l: %f r: %f u: %f d: %f\n", 41 | // potential[n], l, r, u, d); 42 | // ROS_INFO("[Update] cost: %d\n", costs[n]); 43 | 44 | // 找到最小的potential，以及它相邻点的最小potential 45 | float ta, tc; 46 | if (l < r) 47 | tc = l; 48 | else 49 | tc = r; 50 | if (u < d) 51 | ta = u; 52 | else 53 | ta = d; 54 | 55 | float hf = cost; // 描述可通行性的因子 56 | float dc = tc - ta; // tc和tc之间的代价之差 57 | if (dc < 0) // 设置ta为最小的 58 | { 59 | dc = -dc; 60 | ta = tc; 61 | } 62 | 63 | // 计算新的potential, 方法二选一 64 | if (dc >= hf) // ta 和 tc 的代价值差如果太大, 只用ta更新 65 | return ta + hf; 66 | else // two-neighbor interpolation update 67 | { 68 | // use quadratic approximation 69 | // might speed this up through table lookup, but still have to 70 | // do the divide 71 | float d = dc / hf; 72 | float v = -0.2301 * d * d + 0.5307 * d + 0.7040; 73 | return ta + hf * v; 74 | } 75 | } 76 | } 77 | 78 | -------------------------------------------------------------------------------- /map_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | map_server 5 | 1.17.1 6 | 7 | 8 | map_server provides the map_server ROS Node, which offers map data as a ROS Service. It also provides the map_saver command-line utility, which allows dynamically generated maps to be saved to file. 9 | 10 | 11 | Brian Gerkey, Tony Pratkanis 12 | contradict@gmail.com 13 | David V. Lu!! 14 | Michael Ferguson 15 | Aaron Hoy 16 | http://wiki.ros.org/map_server 17 | BSD 18 | 19 | catkin 20 | 21 | bullet 22 | nav_msgs 23 | roscpp 24 | sdl 25 | sdl-image 26 | tf2 27 | yaml-cpp 28 | 29 | roslib 30 | rospy 31 | rostest 32 | rosunit 33 | 34 | -------------------------------------------------------------------------------- /map_server/src/map_server.dox: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | @mainpage 4 | 5 | @htmlinclude manifest.html 6 | 7 | Command-line usage is in the wiki. 8 | */ 9 | 10 | -------------------------------------------------------------------------------- /map_server/test/consumer.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2008, Willow Garage, Inc. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the Willow Garage nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | 35 | from __future__ import print_function 36 | 37 | PKG = 'static_map_server' 38 | NAME = 'consumer' 39 | 40 | import sys 41 | import unittest 42 | import time 43 | 44 | import rospy 45 | import rostest 46 | from nav_msgs.srv import GetMap 47 | 48 | 49 | class TestConsumer(unittest.TestCase): 50 | def __init__(self, *args): 51 | super(TestConsumer, self).__init__(*args) 52 | self.success = False 53 | 54 | def callback(self, data): 55 | print(rospy.get_caller_id(), "I heard %s" % data.data) 56 | self.success = data.data and data.data.startswith('hello world') 57 | rospy.signal_shutdown('test done') 58 | 59 | def test_consumer(self): 60 | rospy.wait_for_service('static_map') 61 | mapsrv = rospy.ServiceProxy('static_map', GetMap) 62 | resp = mapsrv() 63 | self.success = True 64 | print(resp) 65 | while not rospy.is_shutdown() and not self.success: # and time.time() < timeout_t: <== timeout_t doesn't exists?? 66 | time.sleep(0.1) 67 | self.assert_(self.success) 68 | rospy.signal_shutdown('test done') 69 | 70 | if __name__ == '__main__': 71 | rostest.rosrun(PKG, NAME, TestConsumer, sys.argv) 72 | -------------------------------------------------------------------------------- /map_server/test/rtest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /map_server/test/spectrum.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/felderstehost/ros-navigation-noetic/fc1f166d744b5d61ee2815a2bb0d69b3a9b69428/map_server/test/spectrum.png -------------------------------------------------------------------------------- /map_server/test/test_constants.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 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 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | #ifndef MAP_SERVER_TEST_CONSTANTS_H 30 | #define MAP_SERVER_TEST_CONSTANTS_H 31 | 32 | /* Author: Brian Gerkey */ 33 | 34 | /* This file externs global constants shared among tests */ 35 | 36 | extern const unsigned int g_valid_image_width; 37 | extern const unsigned int g_valid_image_height; 38 | extern const char g_valid_image_content[]; 39 | extern const char* g_valid_png_file; 40 | extern const char* g_valid_bmp_file; 41 | extern const float g_valid_image_res; 42 | extern const char* g_spectrum_png_file; 43 | 44 | namespace tmap2 { 45 | extern const char g_valid_image_content[]; 46 | extern const float g_valid_image_res; 47 | extern const unsigned int g_valid_image_width; 48 | extern const unsigned int g_valid_image_height; 49 | } 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /map_server/test/testmap.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/felderstehost/ros-navigation-noetic/fc1f166d744b5d61ee2815a2bb0d69b3a9b69428/map_server/test/testmap.bmp -------------------------------------------------------------------------------- /map_server/test/testmap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/felderstehost/ros-navigation-noetic/fc1f166d744b5d61ee2815a2bb0d69b3a9b69428/map_server/test/testmap.png -------------------------------------------------------------------------------- /map_server/test/testmap.yaml: -------------------------------------------------------------------------------- 1 | image: testmap.png 2 | resolution: 0.1 3 | origin: [2.0, 3.0, 1.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /map_server/test/testmap2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/felderstehost/ros-navigation-noetic/fc1f166d744b5d61ee2815a2bb0d69b3a9b69428/map_server/test/testmap2.png -------------------------------------------------------------------------------- /map_server/test/testmap2.yaml: -------------------------------------------------------------------------------- 1 | image: testmap2.png 2 | resolution: 0.1 3 | origin: [-2.0, -3.0, -1.0] 4 | negate: 1 5 | occupied_thresh: 0.5 6 | free_thresh: 0.2 7 | -------------------------------------------------------------------------------- /move_base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(move_base) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | actionlib 7 | base_local_planner 8 | clear_costmap_recovery 9 | cmake_modules 10 | costmap_2d 11 | dynamic_reconfigure 12 | geometry_msgs 13 | message_generation 14 | move_base_msgs 15 | nav_core 16 | nav_msgs 17 | navfn 18 | pluginlib 19 | roscpp 20 | rospy 21 | rotate_recovery 22 | std_srvs 23 | tf2_geometry_msgs 24 | tf2_ros 25 | ) 26 | find_package(Eigen3 REQUIRED) 27 | add_definitions(${EIGEN3_DEFINITIONS}) 28 | 29 | # dynamic reconfigure 30 | generate_dynamic_reconfigure_options( 31 | cfg/MoveBase.cfg 32 | ) 33 | 34 | catkin_package( 35 | 36 | INCLUDE_DIRS 37 | include 38 | ${EIGEN3_INCLUDE_DIRS} 39 | LIBRARIES move_base 40 | CATKIN_DEPENDS 41 | dynamic_reconfigure 42 | geometry_msgs 43 | move_base_msgs 44 | nav_msgs 45 | roscpp 46 | ) 47 | 48 | include_directories( 49 | include 50 | ${catkin_INCLUDE_DIRS} 51 | ${EIGEN3_INCLUDE_DIRS} 52 | ) 53 | 54 | # move_base 55 | add_library(move_base 56 | src/move_base.cpp 57 | ) 58 | target_link_libraries(move_base 59 | ${Boost_LIBRARIES} 60 | ${catkin_LIBRARIES} 61 | ) 62 | add_dependencies(move_base ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 63 | 64 | add_executable(move_base_node 65 | src/move_base_node.cpp 66 | ) 67 | add_dependencies(move_base_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 68 | target_link_libraries(move_base_node move_base) 69 | set_target_properties(move_base_node PROPERTIES OUTPUT_NAME move_base) 70 | 71 | install( 72 | TARGETS 73 | move_base_node 74 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 75 | ) 76 | 77 | install( 78 | TARGETS 79 | move_base 80 | DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 81 | ) 82 | 83 | ## Mark cpp header files for installation 84 | install(DIRECTORY include/${PROJECT_NAME}/ 85 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 86 | FILES_MATCHING PATTERN "*.h" 87 | ) 88 | -------------------------------------------------------------------------------- /move_base/cfg/MoveBase.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE = 'move_base' 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, str_t, double_t, bool_t, int_t 6 | 7 | gen = ParameterGenerator() 8 | 9 | gen.add("base_global_planner", str_t, 0, "The name of the plugin for the global planner to use with move_base.", "navfn/NavfnROS") 10 | gen.add("base_local_planner", str_t, 0, "The name of the plugin for the local planner to use with move_base.", "base_local_planner/TrajectoryPlannerROS") 11 | 12 | #gen.add("recovery_behaviors", str_t, 0, "A list of recovery behavior plugins to use with move_base.", "[{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: rotate_recovery, type: rotate_recovery/RotateRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}]") 13 | 14 | gen.add("planner_frequency", double_t, 0, "The rate in Hz at which to run the planning loop.", 0, 0, 100) 15 | gen.add("controller_frequency", double_t, 0, "The rate in Hz at which to run the control loop and send velocity commands to the base.", 20, 0, 100) 16 | gen.add("planner_patience", double_t, 0, "How long the planner will wait in seconds in an attempt to find a valid plan before space-clearing operations are performed.", 5.0, 0, 100) 17 | gen.add("controller_patience", double_t, 0, "How long the controller will wait in seconds without receiving a valid control before space-clearing operations are performed.", 5.0, 0, 100) 18 | gen.add("max_planning_retries", int_t, 0, "How many times we will recall the planner in an attempt to find a valid plan before space-clearing operations are performed", -1, -1, 1000) 19 | gen.add("conservative_reset_dist", double_t, 0, "The distance away from the robot in meters at which obstacles will be cleared from the costmap when attempting to clear space in the map.", 3, 0, 50) 20 | 21 | gen.add("recovery_behavior_enabled", bool_t, 0, "Whether or not to enable the move_base recovery behaviors to attempt to clear out space.", True) 22 | # Doesnt exist 23 | gen.add("clearing_rotation_allowed", bool_t, 0, "Determines whether or not the robot will attempt an in-place rotation when attempting to clear out space.", True) 24 | gen.add("shutdown_costmaps", bool_t, 0, "Determines whether or not to shutdown the costmaps of the node when move_base is in an inactive state", False) 25 | 26 | gen.add("oscillation_timeout", double_t, 0, "How long in seconds to allow for oscillation before executing recovery behaviors.", 0.0, 0, 60) 27 | gen.add("oscillation_distance", double_t, 0, "How far in meters the robot must move to be considered not to be oscillating.", 0.5, 0, 10) 28 | 29 | gen.add("make_plan_clear_costmap", bool_t, 0, "Whether or not to clear the global costmap on make_plan service call.", True) 30 | gen.add("make_plan_add_unreachable_goal", bool_t, 0, "Whether or not to add the original goal to the path if it is unreachable in the make_plan service call.", True) 31 | 32 | gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration", False) 33 | exit(gen.generate(PACKAGE, "move_base_node", "MoveBase")) 34 | -------------------------------------------------------------------------------- /move_base/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | move_base 5 | 1.17.1 6 | 7 | 8 | The move_base package provides an implementation of an action (see the actionlib package) that, given a goal in the world, will attempt to reach it with a mobile base. The move_base node links together a global and local planner to accomplish its global navigation task. It supports any global planner adhering to the nav_core::BaseGlobalPlanner interface specified in the nav_core package and any local planner adhering to the nav_core::BaseLocalPlanner interface specified in the nav_core package. The move_base node also maintains two costmaps, one for the global planner, and one for a local planner (see the costmap_2d package) that are used to accomplish navigation tasks. 9 | 10 | 11 | Eitan Marder-Eppstein 12 | contradict@gmail.com 13 | David V. Lu!! 14 | Michael Ferguson 15 | Aaron Hoy 16 | BSD 17 | http://wiki.ros.org/move_base 18 | 19 | catkin 20 | 21 | cmake_modules 22 | message_generation 23 | tf2_geometry_msgs 24 | 25 | actionlib 26 | costmap_2d 27 | dynamic_reconfigure 28 | geometry_msgs 29 | move_base_msgs 30 | nav_core 31 | nav_msgs 32 | pluginlib 33 | roscpp 34 | rospy 35 | std_srvs 36 | tf2_ros 37 | visualization_msgs 38 | 39 | 40 | base_local_planner 41 | clear_costmap_recovery 42 | navfn 43 | rotate_recovery 44 | 45 | message_runtime 46 | 47 | 48 | -------------------------------------------------------------------------------- /move_base/planner_test.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /move_base/src/move_base_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 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 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | 33 | int main(int argc, char** argv){ 34 | ros::init(argc, argv, "move_base_node"); 35 | tf2_ros::Buffer buffer(ros::Duration(10)); 36 | tf2_ros::TransformListener tf(buffer); 37 | 38 | move_base::MoveBase move_base( buffer ); 39 | 40 | //ros::MultiThreadedSpinner s; 41 | ros::spin(); 42 | 43 | return(0); 44 | } 45 | -------------------------------------------------------------------------------- /move_slow_and_clear/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(move_slow_and_clear) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | cmake_modules 7 | costmap_2d 8 | geometry_msgs 9 | nav_core 10 | pluginlib 11 | roscpp 12 | ) 13 | 14 | 15 | find_package(Eigen3 REQUIRED) 16 | remove_definitions(-DDISABLE_LIBUSB-1.0) 17 | find_package(Boost REQUIRED COMPONENTS thread) 18 | include_directories( 19 | include 20 | ${catkin_INCLUDE_DIRS} 21 | ${EIGEN3_INCLUDE_DIRS} 22 | ) 23 | add_definitions(${EIGEN3_DEFINITIONS}) 24 | 25 | catkin_package( 26 | INCLUDE_DIRS include 27 | LIBRARIES move_slow_and_clear 28 | CATKIN_DEPENDS 29 | geometry_msgs 30 | nav_core 31 | pluginlib 32 | roscpp 33 | ) 34 | 35 | add_library(${PROJECT_NAME} src/move_slow_and_clear.cpp) 36 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 37 | target_link_libraries(${PROJECT_NAME} 38 | ${Boost_LIBRARIES} 39 | ${catkin_LIBRARIES} 40 | ) 41 | 42 | ## Install project namespaced headers 43 | install(DIRECTORY include/${PROJECT_NAME}/ 44 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 45 | FILES_MATCHING PATTERN "*.h" 46 | PATTERN ".svn" EXCLUDE) 47 | 48 | install(TARGETS move_slow_and_clear 49 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 50 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 51 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 52 | ) 53 | 54 | install(FILES recovery_plugin.xml 55 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 56 | ) 57 | 58 | 59 | -------------------------------------------------------------------------------- /move_slow_and_clear/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | move_slow_and_clear 5 | 1.17.1 6 | 7 | 8 | move_slow_and_clear 9 | 10 | 11 | Eitan Marder-Eppstein 12 | contradict@gmail.com 13 | David V. Lu!! 14 | Michael Ferguson 15 | Aaron Hoy 16 | BSD 17 | http://wiki.ros.org/move_slow_and_clear 18 | 19 | catkin 20 | 21 | cmake_modules 22 | 23 | costmap_2d 24 | geometry_msgs 25 | nav_core 26 | pluginlib 27 | roscpp 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /move_slow_and_clear/recovery_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A recovery behavior that clears information in the costmap within the circumscribed radius of the robot and then limits the speed of the robot. Note, this recovery behavior is not truly safe, the robot may hit things, it'll just happen at a user-specified speed. Also, this recovery behavior is only compatible with local planners that allow maximum speeds to be set via dynamic reconfigure. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /nav_core/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package nav_core 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.17.1 (2020-08-27) 6 | ------------------- 7 | 8 | 1.17.0 (2020-04-02) 9 | ------------------- 10 | * Merge pull request `#982 `_ from ros-planning/noetic_prep 11 | Noetic Migration 12 | * increase required cmake version 13 | * Contributors: Michael Ferguson 14 | 15 | 1.16.6 (2020-03-18) 16 | ------------------- 17 | 18 | 1.16.5 (2020-03-15) 19 | ------------------- 20 | 21 | 1.16.4 (2020-03-04) 22 | ------------------- 23 | 24 | 1.16.3 (2019-11-15) 25 | ------------------- 26 | 27 | 1.16.2 (2018-07-31) 28 | ------------------- 29 | 30 | 1.16.1 (2018-07-28) 31 | ------------------- 32 | 33 | 1.16.0 (2018-07-25) 34 | ------------------- 35 | * Switch to TF2 `#755 `_ 36 | * unify parameter names between base_local_planner and dwa_local_planner 37 | addresses parts of `#90 `_ 38 | * fix param names of RotateRecovery, closes `#706 `_ 39 | * Contributors: David V. Lu, Michael Ferguson, Vincent Rabaud 40 | 41 | 1.15.2 (2018-03-22) 42 | ------------------- 43 | * Merge pull request `#673 `_ from ros-planning/email_update_lunar 44 | update maintainer email (lunar) 45 | * Merge pull request `#649 `_ from aaronhoy/lunar_add_ahoy 46 | Add myself as a maintainer. 47 | * Contributors: Aaron Hoy, Michael Ferguson 48 | 49 | 1.15.1 (2017-08-14) 50 | ------------------- 51 | 52 | 1.15.0 (2017-08-07) 53 | ------------------- 54 | * convert packages to format2 55 | * makePlan overload must return. 56 | * Contributors: Eric Tappan, Mikael Arguedas 57 | 58 | 1.14.0 (2016-05-20) 59 | ------------------- 60 | 61 | 1.13.1 (2015-10-29) 62 | ------------------- 63 | * Merge pull request `#338 `_ from mikeferguson/planner_review 64 | * fix doxygen, couple style fixes 65 | * Contributors: Michael Ferguson 66 | 67 | 1.13.0 (2015-03-17) 68 | ------------------- 69 | * adding makePlan function with returned cost to base_global_planner 70 | * Contributors: phil0stine 71 | 72 | 1.12.0 (2015-02-04) 73 | ------------------- 74 | * update maintainer email 75 | * Contributors: Michael Ferguson 76 | 77 | 1.11.15 (2015-02-03) 78 | -------------------- 79 | 80 | 1.11.14 (2014-12-05) 81 | -------------------- 82 | 83 | 1.11.13 (2014-10-02) 84 | -------------------- 85 | 86 | 1.11.12 (2014-10-01) 87 | -------------------- 88 | 89 | 1.11.11 (2014-07-23) 90 | -------------------- 91 | 92 | 1.11.10 (2014-06-25) 93 | -------------------- 94 | 95 | 1.11.9 (2014-06-10) 96 | ------------------- 97 | 98 | 1.11.8 (2014-05-21) 99 | ------------------- 100 | 101 | 1.11.7 (2014-05-21) 102 | ------------------- 103 | 104 | 1.11.4 (2013-09-27) 105 | ------------------- 106 | * Package URL Updates 107 | -------------------------------------------------------------------------------- /nav_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(nav_core) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | std_msgs 7 | geometry_msgs 8 | tf2_ros 9 | costmap_2d 10 | ) 11 | 12 | catkin_package( 13 | INCLUDE_DIRS 14 | include 15 | CATKIN_DEPENDS 16 | std_msgs 17 | geometry_msgs 18 | tf2_ros 19 | costmap_2d 20 | ) 21 | 22 | 23 | ## Install project namespaced headers 24 | install(DIRECTORY include/${PROJECT_NAME}/ 25 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 26 | FILES_MATCHING PATTERN "*.h" 27 | PATTERN ".svn" EXCLUDE) 28 | -------------------------------------------------------------------------------- /nav_core/include/nav_core/recovery_behavior.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2009, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | #ifndef NAV_CORE_RECOVERY_BEHAVIOR_H 38 | #define NAV_CORE_RECOVERY_BEHAVIOR_H 39 | 40 | #include 41 | #include 42 | 43 | namespace nav_core { 44 | /** 45 | * @class RecoveryBehavior 46 | * @brief 提供恢复行为的接口，navigation stack调用的所有恢复行为模块都要实现这个接口. 47 | */ 48 | class RecoveryBehavior{ 49 | public: 50 | /** 51 | * @brief RecoveryBehavior 的初始化函数 52 | * @param tf transform listener 的指针 53 | * @param global_costmap 指向 global_costmap 的指针 54 | * @param local_costmap 指向 local_costmap 的指针 55 | */ 56 | virtual void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap) = 0; 57 | 58 | /** 59 | * @brief 运行RecoveryBehavior 60 | */ 61 | virtual void runBehavior() = 0; 62 | 63 | /** 64 | * @brief 接口的虚析构 65 | */ 66 | virtual ~RecoveryBehavior(){} 67 | 68 | protected: 69 | RecoveryBehavior(){} 70 | }; 71 | }; // namespace nav_core 72 | 73 | #endif // NAV_CORE_RECOVERY_BEHAVIOR_H 74 | -------------------------------------------------------------------------------- /nav_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav_core 5 | 1.17.1 6 | 7 | 8 | This package provides common interfaces for navigation specific robot actions. Currently, this package provides the BaseGlobalPlanner, BaseLocalPlanner, and RecoveryBehavior interfaces, which can be used to build actions that can easily swap their planner, local controller, or recovery behavior for new versions adhering to the same interface. 9 | 10 | 11 | Eitan Marder-Eppstein 12 | contradict@gmail.com 13 | David V. Lu!! 14 | Michael Ferguson 15 | Aaron Hoy 16 | BSD 17 | http://wiki.ros.org/nav_core 18 | 19 | catkin 20 | 21 | costmap_2d 22 | geometry_msgs 23 | std_msgs 24 | tf2_ros 25 | 26 | 27 | -------------------------------------------------------------------------------- /navfn/Makefile.orig: -------------------------------------------------------------------------------- 1 | # 2 | # Makefile for navigation function planner 3 | # 4 | 5 | CC = g++ 6 | CXX = g++ 7 | LD = g++ 8 | CPPFLAGS = -Wall -g -O3 -Iinclude -I/usr/local/include -I/usr/local/include/opencv 9 | CFLAGS = -DGCC -msse2 -mpreferred-stack-boundary=4 -O3 -Wall -Iinclude -I/usr/local/include 10 | GCC = g++ 11 | LDFLAGS = -Lbin 12 | SHAREDFLAGS = -shared -Wl,-soname, 13 | LIBS = -lfltk -lnetpbm 14 | 15 | OBJECTS = navfn navwin 16 | 17 | all: bin/navtest 18 | 19 | bin/navtest: obj/navtest.o $(OBJECTS:%=obj/%.o) 20 | $(LD) $(LDFLAGS) -o $@ $(OBJECTS:%=obj/%.o) obj/navtest.o $(LIBS) 21 | 22 | # general cpp command from src->obj 23 | obj/%.o:src/%.cpp 24 | $(GCC) $(CPPFLAGS) -c $< -o $@ 25 | 26 | # general c command from src->obj 27 | obj/%.o:src/%.c 28 | $(GCC) $(CFLAGS) -c $< -o $@ 29 | 30 | obj/navfn.o: include/navfn/navfn.h 31 | 32 | clean: 33 | - rm obj/*.o 34 | - rm bin/navtest 35 | 36 | dist: 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /navfn/bgp_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A implementation of a grid based planner using Dijkstra 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /navfn/include/navfn/potarr_point.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, (Simon) Ye Cheng 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Willow Garage nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #ifndef POTARR_POINT_H_ 38 | #define POTARR_POINT_H_ 39 | 40 | namespace navfn { 41 | struct PotarrPoint { 42 | float x; 43 | float y; 44 | float z; 45 | float pot_value; 46 | }; 47 | } 48 | 49 | #endif 50 | 51 | -------------------------------------------------------------------------------- /navfn/include/navfn/read_pgm_costmap.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 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 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | #ifndef READ_PGM_COSTMAP_H 30 | #define READ_PGM_COSTMAP_H 31 | 32 | // is true for ROS-generated raw cost maps 33 | COSTTYPE *readPGM(const char *fname, int *width, int *height, bool raw = false); 34 | 35 | #endif // READ_PGM_COSTMAP_H 36 | -------------------------------------------------------------------------------- /navfn/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | navfn 5 | 1.17.1 6 | 7 | 8 | navfn provides a fast interpolated navigation function that can be used to create plans for 9 | a mobile base. The planner assumes a circular robot and operates on a costmap to find a 10 | minimum cost plan from a start point to an end point in a grid. The navigation function is 11 | computed with Dijkstra's algorithm, but support for an A* heuristic may also be added in the 12 | near future. navfn also provides a ROS wrapper for the navfn planner that adheres to the 13 | nav_core::BaseGlobalPlanner interface specified in nav_core. 14 | 15 | 16 | Kurt Konolige 17 | Eitan Marder-Eppstein 18 | contradict@gmail.com 19 | David V. Lu!! 20 | Michael Ferguson 21 | Aaron Hoy 22 | BSD 23 | http://wiki.ros.org/navfn 24 | 25 | catkin 26 | 27 | cmake_modules 28 | message_generation 29 | netpbm 30 | 31 | costmap_2d 32 | geometry_msgs 33 | nav_core 34 | nav_msgs 35 | pluginlib 36 | rosconsole 37 | roscpp 38 | sensor_msgs 39 | tf2_ros 40 | visualization_msgs 41 | 42 | message_runtime 43 | 44 | rosunit 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /navfn/src/navtest/navwin.h: -------------------------------------------------------------------------------- 1 | // 2 | // drawing fns for nav fn 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | namespace navfn { 18 | class NavWin 19 | : public Fl_Double_Window 20 | { 21 | public: 22 | NavWin(int w, int h, const char *name); 23 | ~NavWin(); 24 | 25 | int nw,nh; // width and height of image 26 | int pw,ph; // width and height of pot field 27 | int dec, inc; // decimation or expansion for display 28 | 29 | float maxval; // max potential value 30 | void drawPot(NavFn *nav); // draw everything... 31 | 32 | void drawOverlay(); 33 | 34 | uchar *im; // image for drawing 35 | int *pc, *pn, *po; // priority buffers 36 | int pce, pne, poe; // buffer sizes 37 | int goal[2]; 38 | int start[2]; 39 | int *path; // path buffer, cell indices 40 | int pathlen; // how many we have 41 | int pathbuflen; // how big the path buffer is 42 | 43 | void draw(); // draw the image 44 | }; 45 | }; 46 | -------------------------------------------------------------------------------- /navfn/srv/MakeNavPlan.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped start 2 | geometry_msgs/PoseStamped goal 3 | --- 4 | 5 | uint8 plan_found 6 | string error_message 7 | 8 | # if plan_found is true, this is an array of waypoints from start to goal, where the first one equals start and the last one equals goal 9 | geometry_msgs/PoseStamped[] path 10 | -------------------------------------------------------------------------------- /navfn/srv/SetCostmap.srv: -------------------------------------------------------------------------------- 1 | uint8[] costs 2 | uint16 height 3 | uint16 width 4 | --- -------------------------------------------------------------------------------- /navfn/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | catkin_add_gtest(path_calc_test path_calc_test.cpp ../src/read_pgm_costmap.cpp) 2 | target_link_libraries(path_calc_test navfn netpbm) 3 | -------------------------------------------------------------------------------- /navfn/test/willow_costmap.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/felderstehost/ros-navigation-noetic/fc1f166d744b5d61ee2815a2bb0d69b3a9b69428/navfn/test/willow_costmap.pgm -------------------------------------------------------------------------------- /navigation/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package navigation 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.17.1 (2020-08-27) 6 | ------------------- 7 | 8 | 1.17.0 (2020-04-02) 9 | ------------------- 10 | * Merge pull request `#982 `_ from ros-planning/noetic_prep 11 | Noetic Migration 12 | * increase required cmake version 13 | * Contributors: Michael Ferguson 14 | 15 | 1.16.6 (2020-03-18) 16 | ------------------- 17 | 18 | 1.16.5 (2020-03-15) 19 | ------------------- 20 | 21 | 1.16.4 (2020-03-04) 22 | ------------------- 23 | 24 | 1.16.3 (2019-11-15) 25 | ------------------- 26 | 27 | 1.16.2 (2018-07-31) 28 | ------------------- 29 | 30 | 1.16.1 (2018-07-28) 31 | ------------------- 32 | 33 | 1.16.0 (2018-07-25) 34 | ------------------- 35 | * remove robot_pose_ekf, see `#701 `_ 36 | * Contributors: Michael Ferguson 37 | 38 | 1.15.2 (2018-03-22) 39 | ------------------- 40 | * Merge pull request `#673 `_ from ros-planning/email_update_lunar 41 | update maintainer email (lunar) 42 | * Merge pull request `#649 `_ from aaronhoy/lunar_add_ahoy 43 | Add myself as a maintainer. 44 | * Contributors: Aaron Hoy, Michael Ferguson 45 | 46 | 1.15.1 (2017-08-14) 47 | ------------------- 48 | 49 | 1.15.0 (2017-08-07) 50 | ------------------- 51 | * convert packages to format2 52 | * Contributors: Mikael Arguedas 53 | 54 | 1.14.0 (2016-05-20) 55 | ------------------- 56 | 57 | 1.13.1 (2015-10-29) 58 | ------------------- 59 | 60 | 1.13.0 (2015-03-17) 61 | ------------------- 62 | 63 | 1.12.0 (2015-02-04) 64 | ------------------- 65 | * update maintainer email 66 | * Contributors: Michael Ferguson 67 | 68 | 1.11.15 (2015-02-03) 69 | -------------------- 70 | 71 | 1.11.14 (2014-12-05) 72 | -------------------- 73 | * add global planner run depend in meta package. 74 | * Contributors: Jihoon Lee 75 | 76 | 1.11.13 (2014-10-02) 77 | -------------------- 78 | 79 | 1.11.12 (2014-10-01) 80 | -------------------- 81 | 82 | 1.11.11 (2014-07-23) 83 | -------------------- 84 | 85 | 1.11.10 (2014-06-25) 86 | -------------------- 87 | 88 | 1.11.9 (2014-06-10) 89 | ------------------- 90 | 91 | 1.11.8 (2014-05-21) 92 | ------------------- 93 | 94 | 1.11.7 (2014-05-21) 95 | ------------------- 96 | 97 | 1.11.4 (2013-09-27) 98 | ------------------- 99 | * Package URL Updates 100 | -------------------------------------------------------------------------------- /navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(navigation) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /navigation/README.rst: -------------------------------------------------------------------------------- 1 | ROS Navigation Stack 2 | -------------------- 3 | 4 | A 2D navigation stack that takes in information from odometry, sensor 5 | streams, and a goal pose and outputs safe velocity commands that are sent 6 | to a mobile base. 7 | 8 | Related stacks: 9 | 10 | * http://github.com/ros-planning/navigation_tutorials 11 | * http://github.com/ros-planning/navigation_experimental 12 | -------------------------------------------------------------------------------- /navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | navigation 5 | 1.17.1 6 | 7 | A 2D navigation stack that takes in information from odometry, sensor 8 | streams, and a goal pose and outputs safe velocity commands that are sent 9 | to a mobile base. 10 | 11 | Michael Ferguson 12 | David V. Lu!! 13 | Aaron Hoy 14 | contradict@gmail.com 15 | Eitan Marder-Eppstein 16 | BSD,LGPL,LGPL (amcl) 17 | http://wiki.ros.org/navigation 18 | 19 | catkin 20 | 21 | amcl 22 | base_local_planner 23 | carrot_planner 24 | clear_costmap_recovery 25 | costmap_2d 26 | dwa_local_planner 27 | fake_localization 28 | global_planner 29 | map_server 30 | move_base 31 | move_base_msgs 32 | move_slow_and_clear 33 | navfn 34 | nav_core 35 | rotate_recovery 36 | voxel_grid 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /rotate_recovery/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(rotate_recovery) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | angles 7 | base_local_planner 8 | cmake_modules 9 | costmap_2d 10 | geometry_msgs 11 | nav_core 12 | pluginlib 13 | roscpp 14 | tf2 15 | tf2_geometry_msgs 16 | tf2_ros 17 | ) 18 | 19 | find_package(Eigen3 REQUIRED) 20 | include_directories( 21 | include 22 | ${catkin_INCLUDE_DIRS} 23 | ${EIGEN3_INCLUDE_DIRS} 24 | ) 25 | add_definitions(${EIGEN3_DEFINITIONS}) 26 | 27 | catkin_package( 28 | INCLUDE_DIRS include 29 | LIBRARIES rotate_recovery 30 | CATKIN_DEPENDS 31 | costmap_2d 32 | geometry_msgs 33 | nav_core 34 | pluginlib 35 | roscpp 36 | tf2 37 | tf2_ros 38 | ) 39 | 40 | add_library(rotate_recovery src/rotate_recovery.cpp) 41 | add_dependencies(rotate_recovery ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 42 | target_link_libraries(rotate_recovery ${catkin_LIBRARIES}) 43 | 44 | install(TARGETS rotate_recovery 45 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 46 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 47 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 48 | ) 49 | 50 | install(DIRECTORY include/${PROJECT_NAME}/ 51 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 52 | FILES_MATCHING PATTERN "*.h" 53 | ) 54 | 55 | install(FILES rotate_plugin.xml 56 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 57 | ) 58 | 59 | -------------------------------------------------------------------------------- /rotate_recovery/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rotate_recovery 5 | 1.17.1 6 | 7 | 8 | This package provides a recovery behavior for the navigation stack that attempts to clear space by performing a 360 degree rotation of the robot. 9 | 10 | 11 | Eitan Marder-Eppstein 12 | contradict@gmail.com 13 | David V. Lu!! 14 | Michael Ferguson 15 | Aaron Hoy 16 | BSD 17 | http://wiki.ros.org/rotate_recovery 18 | 19 | catkin 20 | 21 | angles 22 | base_local_planner 23 | cmake_modules 24 | 25 | costmap_2d 26 | eigen 27 | geometry_msgs 28 | nav_core 29 | pluginlib 30 | roscpp 31 | tf2 32 | tf2_geometry_msgs 33 | tf2_ros 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /rotate_recovery/rotate_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A recovery behavior that performs a 360 degree in-place rotation to attempt to clear out space. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /voxel_grid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(voxel_grid) 3 | 4 | include(CheckIncludeFile) 5 | 6 | find_package(catkin REQUIRED 7 | COMPONENTS 8 | roscpp 9 | ) 10 | 11 | catkin_package( 12 | INCLUDE_DIRS 13 | include 14 | LIBRARIES 15 | voxel_grid 16 | CATKIN_DEPENDS 17 | roscpp 18 | ) 19 | 20 | include_directories(include ${catkin_INCLUDE_DIRS}) 21 | 22 | check_include_file(sys/time.h HAVE_SYS_TIME_H) 23 | if (HAVE_SYS_TIME_H) 24 | add_definitions(-DHAVE_SYS_TIME_H) 25 | endif (HAVE_SYS_TIME_H) 26 | 27 | add_library(voxel_grid src/voxel_grid.cpp) 28 | add_dependencies(voxel_grid ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 29 | target_link_libraries(voxel_grid ${catkin_LIBRARIES}) 30 | 31 | install(TARGETS voxel_grid 32 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 33 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 34 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 35 | ) 36 | 37 | install(DIRECTORY include/${PROJECT_NAME}/ 38 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 39 | ) 40 | 41 | if(CATKIN_ENABLE_TESTING) 42 | catkin_add_gtest(voxel_grid_tests test/voxel_grid_tests.cpp) 43 | target_link_libraries(voxel_grid_tests 44 | voxel_grid 45 | ${catkin_LIBRARIES} 46 | ) 47 | endif() 48 | -------------------------------------------------------------------------------- /voxel_grid/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | voxel_grid 5 | 1.17.1 6 | 7 | 8 | voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. 9 | 10 | 11 | Eitan Marder-Eppstein, Eric Berger 12 | contradict@gmail.com 13 | David V. Lu!! 14 | Michael Ferguson 15 | Aaron Hoy 16 | BSD 17 | http://wiki.ros.org/voxel_grid 18 | 19 | catkin 20 | 21 | roscpp 22 | 23 | rosunit 24 | 25 | --------------------------------------------------------------------------------