├── .gitignore ├── base_local_planner ├── msg │ └── Position2DInt.msg ├── src │ ├── local_planner_limits │ │ ├── .gitignore │ │ └── __init__.py │ ├── twirling_cost_function.cpp │ ├── prefer_forward_cost_function.cpp │ └── map_cell.cpp ├── setup.py ├── cfg │ └── LocalPlannerLimits.cfg ├── test │ ├── gtest_main.cpp │ ├── trajectory_generator_test.cpp │ └── wavefront_map_accessor.h ├── blp_plugin.xml ├── include │ └── base_local_planner │ │ ├── trajectory_inc.h │ │ ├── planar_laser_scan.h │ │ ├── prefer_forward_cost_function.h │ │ ├── map_cell.h │ │ ├── trajectory_sample_generator.h │ │ ├── twirling_cost_function.h │ │ └── trajectory_search.h └── package.xml ├── navfn ├── srv │ ├── SetCostmap.srv │ └── MakeNavPlan.srv ├── test │ ├── willow_costmap.pgm │ └── CMakeLists.txt ├── bgp_plugin.xml ├── Makefile.orig ├── include │ └── navfn │ │ ├── navwin.h │ │ ├── read_pgm_costmap.h │ │ └── potarr_point.h └── package.xml ├── map_server ├── test │ ├── testmap.bmp │ ├── testmap.png │ ├── testmap.yaml │ ├── rtest.xml │ ├── test_constants.h │ ├── test_constants.cpp │ └── consumer.py ├── src │ └── map_server.dox └── package.xml ├── costmap_2d ├── test │ ├── TenByTen.pgm │ ├── TenByTen.yaml │ ├── static_tests.launch │ ├── obstacle_tests.launch │ ├── inflation_tests.launch │ ├── simple_driving_test.xml │ ├── costmap_params.yaml │ ├── footprint_tests.launch │ └── array_parser_test.cpp ├── msg │ └── VoxelGrid.msg ├── cfg │ ├── GenericPlugin.cfg │ ├── InflationPlugin.cfg │ ├── ObstaclePlugin.cfg │ ├── VoxelPlugin.cfg │ └── Costmap2D.cfg ├── launch │ ├── example.launch │ └── example_params.yaml ├── costmap_plugins.xml ├── src │ ├── layer.cpp │ └── costmap_2d_node.cpp ├── include │ └── costmap_2d │ │ ├── cost_values.h │ │ ├── array_parser.h │ │ └── costmap_math.h └── package.xml ├── navigation ├── CMakeLists.txt ├── README.rst ├── package.xml └── CHANGELOG.rst ├── global_planner ├── bgp_plugin.xml ├── package.xml ├── cfg │ └── GlobalPlanner.cfg ├── CMakeLists.txt └── include │ └── global_planner │ ├── grid_path.h │ ├── quadratic_calculator.h │ ├── traceback.h │ ├── astar.h │ └── orientation_filter.h ├── carrot_planner ├── bgp_plugin.xml ├── CMakeLists.txt ├── package.xml └── CHANGELOG.rst ├── rotate_recovery ├── rotate_plugin.xml ├── CMakeLists.txt └── package.xml ├── amcl ├── include │ └── amcl │ │ ├── pf │ │ ├── eig3.h │ │ ├── pf_pdf.h │ │ └── pf_vector.h │ │ └── sensors │ │ ├── amcl_odom.h │ │ └── amcl_sensor.h ├── examples │ ├── amcl_omni.launch │ └── amcl_diff.launch ├── package.xml ├── test │ ├── set_pose.py │ ├── texas_greenroom_loop.xml │ ├── texas_greenroom_loop_corrected.xml │ ├── texas_willow_hallway_loop_corrected.xml │ ├── small_loop_prf.xml │ ├── global_localization_stage.xml │ ├── rosie_multilaser.xml │ ├── basic_localization_stage.xml │ ├── set_initial_pose.xml │ ├── small_loop_crazy_driving_prg.xml │ ├── small_loop_crazy_driving_prg_corrected.xml │ ├── texas_willow_hallway_loop.xml │ └── set_initial_pose_delayed.xml └── src │ └── amcl │ ├── map │ └── map.c │ └── sensors │ └── amcl_sensor.cpp ├── dwa_local_planner ├── blp_plugin.xml ├── CMakeLists.txt ├── package.xml └── cfg │ └── DWAPlanner.cfg ├── clear_costmap_recovery ├── ccr_plugin.xml ├── test │ ├── params.yaml │ └── clear_tests.launch ├── package.xml └── CMakeLists.txt ├── move_base ├── planner_test.xml ├── src │ └── move_base_node.cpp ├── CMakeLists.txt ├── package.xml └── cfg │ └── MoveBase.cfg ├── move_slow_and_clear ├── recovery_plugin.xml ├── package.xml └── CMakeLists.txt ├── nav_core ├── CMakeLists.txt ├── package.xml └── CHANGELOG.rst ├── README.md ├── .travis.yml ├── voxel_grid ├── CMakeLists.txt ├── package.xml └── CHANGELOG.rst └── fake_localization ├── package.xml ├── CMakeLists.txt ├── static_odom_broadcaster.py └── CHANGELOG.rst /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.project 3 | *.cproject 4 | 5 | -------------------------------------------------------------------------------- /base_local_planner/msg/Position2DInt.msg: -------------------------------------------------------------------------------- 1 | int64 x 2 | int64 y -------------------------------------------------------------------------------- /base_local_planner/src/local_planner_limits/.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | -------------------------------------------------------------------------------- /navfn/srv/SetCostmap.srv: -------------------------------------------------------------------------------- 1 | uint8[] costs 2 | uint16 height 3 | uint16 width 4 | --- -------------------------------------------------------------------------------- /map_server/test/testmap.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ns130291/navigation/melodic-devel/map_server/test/testmap.bmp -------------------------------------------------------------------------------- /map_server/test/testmap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ns130291/navigation/melodic-devel/map_server/test/testmap.png -------------------------------------------------------------------------------- /costmap_2d/test/TenByTen.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ns130291/navigation/melodic-devel/costmap_2d/test/TenByTen.pgm -------------------------------------------------------------------------------- /navfn/test/willow_costmap.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ns130291/navigation/melodic-devel/navfn/test/willow_costmap.pgm -------------------------------------------------------------------------------- /navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(navigation) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /map_server/test/rtest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /costmap_2d/test/static_tests.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /costmap_2d/test/obstacle_tests.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /navfn/bgp_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A implementation of a grid based planner using Dijkstra 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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/test/inflation_tests.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /global_planner/bgp_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A implementation of a grid based planner using Dijkstras or A* 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /clear_costmap_recovery/test/clear_tests.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /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/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 | -------------------------------------------------------------------------------- /move_base/planner_test.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 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 | -------------------------------------------------------------------------------- /costmap_2d/test/simple_driving_test.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | // backward motions bad on a robot without backward sensors 17 | if (traj.xv_ < 0.0) { 18 | return penalty_; 19 | } 20 | // strafing motions also bad on such a robot 21 | if (traj.xv_ < 0.1 && fabs(traj.thetav_) < 0.2) { 22 | return penalty_; 23 | } 24 | // the more we rotate, the less we progress forward 25 | return fabs(traj.thetav_) * 10; 26 | } 27 | 28 | } /* namespace base_local_planner */ 29 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # This config file for Travis CI utilizes ros-industrial/industrial_ci package. 2 | # For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst 3 | sudo: required 4 | dist: trusty 5 | services: 6 | - docker 7 | language: generic 8 | compiler: 9 | - gcc 10 | notifications: 11 | email: 12 | on_success: always 13 | on_failure: always 14 | recipients: 15 | - gm130s@gmail.com 16 | env: 17 | matrix: 18 | - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu USE_DEB=true 19 | - ROS_DISTRO="kinetic" PRERELEASE=true PRERELEASE_DOWNSTREAM_DEPTH=0 20 | matrix: 21 | allow_failures: 22 | - env: ROS_DISTRO="kinetic" PRERELEASE=true PRERELEASE_DOWNSTREAM_DEPTH=0 23 | install: 24 | - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config && source .ci_config/travis.sh 25 | -------------------------------------------------------------------------------- /costmap_2d/launch/example.launch: -------------------------------------------------------------------------------- 1 | 2 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /voxel_grid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(voxel_grid) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | roscpp 7 | ) 8 | 9 | catkin_package( 10 | INCLUDE_DIRS 11 | include 12 | LIBRARIES 13 | voxel_grid 14 | CATKIN_DEPENDS 15 | roscpp 16 | ) 17 | 18 | include_directories(include ${catkin_INCLUDE_DIRS}) 19 | 20 | add_library(voxel_grid src/voxel_grid.cpp) 21 | add_dependencies(voxel_grid ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 22 | target_link_libraries(voxel_grid ${catkin_LIBRARIES}) 23 | 24 | install(TARGETS voxel_grid 25 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 26 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 27 | ) 28 | 29 | install(DIRECTORY include/${PROJECT_NAME}/ 30 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 31 | ) 32 | 33 | if(CATKIN_ENABLE_TESTING) 34 | catkin_add_gtest(voxel_grid_tests test/voxel_grid_tests.cpp) 35 | target_link_libraries(voxel_grid_tests 36 | voxel_grid 37 | ${catkin_LIBRARIES} 38 | ) 39 | endif() 40 | -------------------------------------------------------------------------------- /fake_localization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | fake_localization 5 | 1.16.2 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 | -------------------------------------------------------------------------------- /move_slow_and_clear/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | move_slow_and_clear 5 | 1.16.2 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 | -------------------------------------------------------------------------------- /fake_localization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 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 | ) 15 | 16 | 17 | find_package(Boost REQUIRED COMPONENTS signals) 18 | 19 | catkin_package( 20 | CATKIN_DEPENDS 21 | geometry_msgs 22 | nav_msgs 23 | roscpp 24 | rospy 25 | ) 26 | 27 | include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 28 | 29 | add_executable(fake_localization fake_localization.cpp) 30 | target_link_libraries(fake_localization 31 | ${catkin_LIBRARIES} 32 | ${Boost_LIBRARIES} 33 | ) 34 | add_dependencies(fake_localization ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 35 | 36 | install( 37 | PROGRAMS 38 | static_odom_broadcaster.py 39 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 40 | ) 41 | 42 | install( 43 | TARGETS 44 | fake_localization 45 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 46 | ) 47 | -------------------------------------------------------------------------------- /navfn/include/navfn/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 "navfn.h" 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 | -------------------------------------------------------------------------------- /global_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | global_planner 5 | 1.16.2 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 | -------------------------------------------------------------------------------- /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/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 | -------------------------------------------------------------------------------- /nav_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav_core 5 | 1.16.2 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 | -------------------------------------------------------------------------------- /carrot_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(carrot_planner) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | base_local_planner 7 | costmap_2d 8 | nav_core 9 | pluginlib 10 | roscpp 11 | tf2 12 | tf2_geometry_msgs 13 | tf2_ros 14 | ) 15 | 16 | include_directories( 17 | include 18 | ${catkin_INCLUDE_DIRS} 19 | ) 20 | add_definitions(${EIGEN3_DEFINITIONS}) 21 | 22 | catkin_package( 23 | INCLUDE_DIRS include 24 | LIBRARIES carrot_planner 25 | CATKIN_DEPENDS 26 | base_local_planner 27 | costmap_2d 28 | nav_core 29 | pluginlib 30 | roscpp 31 | tf2 32 | tf2_ros 33 | ) 34 | 35 | add_library(carrot_planner src/carrot_planner.cpp) 36 | add_dependencies(carrot_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 37 | target_link_libraries(carrot_planner 38 | ${catkin_LIBRARIES} 39 | ) 40 | 41 | install(TARGETS carrot_planner 42 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 43 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 44 | ) 45 | 46 | install(DIRECTORY include/${PROJECT_NAME}/ 47 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 48 | PATTERN ".svn" EXCLUDE 49 | ) 50 | 51 | install(FILES bgp_plugin.xml 52 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 53 | ) 54 | 55 | 56 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /voxel_grid/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | voxel_grid 5 | 1.16.2 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 | -------------------------------------------------------------------------------- /clear_costmap_recovery/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | clear_costmap_recovery 5 | 1.16.2 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 | -------------------------------------------------------------------------------- /carrot_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | carrot_planner 5 | 1.16.2 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 | base_local_planner 25 | costmap_2d 26 | eigen 27 | nav_core 28 | pluginlib 29 | roscpp 30 | tf2 31 | tf2_ros 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /rotate_recovery/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 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 | ) 48 | 49 | install(DIRECTORY include/${PROJECT_NAME}/ 50 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 51 | FILES_MATCHING PATTERN "*.h" 52 | ) 53 | 54 | install(FILES rotate_plugin.xml 55 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 56 | ) 57 | 58 | -------------------------------------------------------------------------------- /map_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | map_server 5 | 1.16.2 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 | rospy 30 | rostest 31 | rosunit 32 | 33 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /rotate_recovery/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rotate_recovery 5 | 1.16.2 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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /move_slow_and_clear/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 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 | ) 52 | 53 | install(FILES recovery_plugin.xml 54 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 55 | ) 56 | 57 | 58 | -------------------------------------------------------------------------------- /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 be 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 | -------------------------------------------------------------------------------- /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 | 36 | 37 | -------------------------------------------------------------------------------- /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 | 36 | 37 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | navigation 5 | 1.16.2 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 | -------------------------------------------------------------------------------- /amcl/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | amcl 5 | 1.16.2 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 | dynamic_reconfigure 32 | geometry_msgs 33 | nav_msgs 34 | rosbag 35 | roscpp 36 | sensor_msgs 37 | std_srvs 38 | tf2 39 | tf2_msgs 40 | tf2_ros 41 | 42 | map_server 43 | rostest 44 | python_orocos_kdl 45 | 46 | -------------------------------------------------------------------------------- /dwa_local_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 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 | ) 57 | 58 | install(FILES blp_plugin.xml 59 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 60 | ) 61 | 62 | install(DIRECTORY include/${PROJECT_NAME}/ 63 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 64 | PATTERN ".svn" EXCLUDE 65 | ) 66 | -------------------------------------------------------------------------------- /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/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 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} planner 65 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 66 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 67 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 68 | 69 | install(DIRECTORY include/${PROJECT_NAME}/ 70 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 71 | PATTERN ".svn" EXCLUDE) 72 | 73 | install(FILES bgp_plugin.xml 74 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 75 | -------------------------------------------------------------------------------- /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 = 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 | -------------------------------------------------------------------------------- /clear_costmap_recovery/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 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 | ) 57 | 58 | install(FILES ccr_plugin.xml 59 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 60 | ) 61 | 62 | install(DIRECTORY include/${PROJECT_NAME}/ 63 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 64 | PATTERN ".svn" EXCLUDE 65 | ) 66 | -------------------------------------------------------------------------------- /navigation/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package navigation 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.16.2 (2018-07-31) 6 | ------------------- 7 | 8 | 1.16.1 (2018-07-28) 9 | ------------------- 10 | 11 | 1.16.0 (2018-07-25) 12 | ------------------- 13 | * remove robot_pose_ekf, see `#701 `_ 14 | * Contributors: Michael Ferguson 15 | 16 | 1.15.2 (2018-03-22) 17 | ------------------- 18 | * Merge pull request `#673 `_ from ros-planning/email_update_lunar 19 | update maintainer email (lunar) 20 | * Merge pull request `#649 `_ from aaronhoy/lunar_add_ahoy 21 | Add myself as a maintainer. 22 | * Contributors: Aaron Hoy, Michael Ferguson 23 | 24 | 1.15.1 (2017-08-14) 25 | ------------------- 26 | 27 | 1.15.0 (2017-08-07) 28 | ------------------- 29 | * convert packages to format2 30 | * Contributors: Mikael Arguedas 31 | 32 | 1.14.0 (2016-05-20) 33 | ------------------- 34 | 35 | 1.13.1 (2015-10-29) 36 | ------------------- 37 | 38 | 1.13.0 (2015-03-17) 39 | ------------------- 40 | 41 | 1.12.0 (2015-02-04) 42 | ------------------- 43 | * update maintainer email 44 | * Contributors: Michael Ferguson 45 | 46 | 1.11.15 (2015-02-03) 47 | -------------------- 48 | 49 | 1.11.14 (2014-12-05) 50 | -------------------- 51 | * add global planner run depend in meta package. 52 | * Contributors: Jihoon Lee 53 | 54 | 1.11.13 (2014-10-02) 55 | -------------------- 56 | 57 | 1.11.12 (2014-10-01) 58 | -------------------- 59 | 60 | 1.11.11 (2014-07-23) 61 | -------------------- 62 | 63 | 1.11.10 (2014-06-25) 64 | -------------------- 65 | 66 | 1.11.9 (2014-06-10) 67 | ------------------- 68 | 69 | 1.11.8 (2014-05-21) 70 | ------------------- 71 | 72 | 1.11.7 (2014-05-21) 73 | ------------------- 74 | 75 | 1.11.4 (2013-09-27) 76 | ------------------- 77 | * Package URL Updates 78 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /amcl/test/texas_greenroom_loop.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 | 37 | 38 | -------------------------------------------------------------------------------- /amcl/test/texas_greenroom_loop_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 | 37 | 38 | -------------------------------------------------------------------------------- /amcl/test/texas_willow_hallway_loop_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 | 37 | 38 | -------------------------------------------------------------------------------- /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_base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 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 | CATKIN_DEPENDS 36 | dynamic_reconfigure 37 | geometry_msgs 38 | move_base_msgs 39 | nav_msgs 40 | roscpp 41 | ) 42 | 43 | include_directories( 44 | include 45 | ${catkin_INCLUDE_DIRS} 46 | ${EIGEN3_INCLUDE_DIRS} 47 | ) 48 | 49 | # move_base 50 | add_library(move_base 51 | src/move_base.cpp 52 | ) 53 | target_link_libraries(move_base 54 | ${Boost_LIBRARIES} 55 | ${catkin_LIBRARIES} 56 | ) 57 | add_dependencies(move_base ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 58 | 59 | add_executable(move_base_node 60 | src/move_base_node.cpp 61 | ) 62 | add_dependencies(move_base_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 63 | target_link_libraries(move_base_node move_base) 64 | set_target_properties(move_base_node PROPERTIES OUTPUT_NAME move_base) 65 | 66 | install( 67 | TARGETS 68 | move_base 69 | move_base_node 70 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 71 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 72 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 73 | ) 74 | 75 | ## Mark cpp header files for installation 76 | install(DIRECTORY include/${PROJECT_NAME}/ 77 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 78 | FILES_MATCHING PATTERN "*.h" 79 | ) 80 | -------------------------------------------------------------------------------- /dwa_local_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | dwa_local_planner 5 | 1.16.2 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 | -------------------------------------------------------------------------------- /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/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | navfn 5 | 1.16.2 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 | -------------------------------------------------------------------------------- /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 | 43 | #endif 44 | -------------------------------------------------------------------------------- /fake_localization/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package fake_localization 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.16.2 (2018-07-31) 6 | ------------------- 7 | 8 | 1.16.1 (2018-07-28) 9 | ------------------- 10 | 11 | 1.16.0 (2018-07-25) 12 | ------------------- 13 | * Merge pull request `#690 `_ from ros-planning/lunar_609 14 | switch fake_localization to tf2. 15 | * Contributors: Michael Ferguson, Vincent Rabaud 16 | 17 | 1.15.2 (2018-03-22) 18 | ------------------- 19 | * Merge pull request `#673 `_ from ros-planning/email_update_lunar 20 | update maintainer email (lunar) 21 | * Merge pull request `#649 `_ from aaronhoy/lunar_add_ahoy 22 | Add myself as a maintainer. 23 | * Contributors: Aaron Hoy, Michael Ferguson 24 | 25 | 1.15.1 (2017-08-14) 26 | ------------------- 27 | 28 | 1.15.0 (2017-08-07) 29 | ------------------- 30 | * convert packages to format2 31 | * Fix CMakeLists + package.xmls (`#548 `_) 32 | * Contributors: Martin Günther, Mikael Arguedas, Vincent Rabaud 33 | 34 | 1.14.0 (2016-05-20) 35 | ------------------- 36 | 37 | 1.13.1 (2015-10-29) 38 | ------------------- 39 | * More tolerant initial pose transform lookup. 40 | * Contributors: Daniel Stonier 41 | 42 | 1.13.0 (2015-03-17) 43 | ------------------- 44 | 45 | 1.12.0 (2015-02-04) 46 | ------------------- 47 | * update maintainer email 48 | * Contributors: Michael Ferguson 49 | 50 | 1.11.15 (2015-02-03) 51 | -------------------- 52 | 53 | 1.11.14 (2014-12-05) 54 | -------------------- 55 | 56 | 1.11.13 (2014-10-02) 57 | -------------------- 58 | 59 | 1.11.12 (2014-10-01) 60 | -------------------- 61 | 62 | 1.11.11 (2014-07-23) 63 | -------------------- 64 | 65 | 1.11.10 (2014-06-25) 66 | -------------------- 67 | 68 | 1.11.9 (2014-06-10) 69 | ------------------- 70 | 71 | 1.11.8 (2014-05-21) 72 | ------------------- 73 | 74 | 1.11.7 (2014-05-21) 75 | ------------------- 76 | 77 | 1.11.4 (2013-09-27) 78 | ------------------- 79 | * Package URL Updates 80 | * amcl_pose and particle cloud are now published latched 81 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /voxel_grid/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package voxel_grid 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.16.2 (2018-07-31) 6 | ------------------- 7 | 8 | 1.16.1 (2018-07-28) 9 | ------------------- 10 | 11 | 1.16.0 (2018-07-25) 12 | ------------------- 13 | 14 | 1.15.2 (2018-03-22) 15 | ------------------- 16 | * Merge pull request `#673 `_ from ros-planning/email_update_lunar 17 | update maintainer email (lunar) 18 | * Merge pull request `#649 `_ from aaronhoy/lunar_add_ahoy 19 | Add myself as a maintainer. 20 | * Contributors: Aaron Hoy, Michael Ferguson 21 | 22 | 1.15.1 (2017-08-14) 23 | ------------------- 24 | 25 | 1.15.0 (2017-08-07) 26 | ------------------- 27 | * convert packages to format2 28 | * Fix CMakeLists + package.xmls (`#548 `_) 29 | * Contributors: Martin Günther, Mikael Arguedas 30 | 31 | 1.14.0 (2016-05-20) 32 | ------------------- 33 | 34 | 1.13.1 (2015-10-29) 35 | ------------------- 36 | 37 | 1.13.0 (2015-03-17) 38 | ------------------- 39 | 40 | 1.12.0 (2015-02-04) 41 | ------------------- 42 | * update maintainer email 43 | * Contributors: Michael Ferguson 44 | 45 | 1.11.15 (2015-02-03) 46 | -------------------- 47 | * Add ARCHIVE_DESTINATION for static builds 48 | * Contributors: Gary Servin 49 | 50 | 1.11.14 (2014-12-05) 51 | -------------------- 52 | * remove old test code 53 | * fixup voxel_grid.h formatting (whitespace changes ONLY) 54 | * Contributors: Michael Ferguson 55 | 56 | 1.11.13 (2014-10-02) 57 | -------------------- 58 | 59 | 1.11.12 (2014-10-01) 60 | -------------------- 61 | 62 | 1.11.11 (2014-07-23) 63 | -------------------- 64 | 65 | 1.11.10 (2014-06-25) 66 | -------------------- 67 | 68 | 1.11.9 (2014-06-10) 69 | ------------------- 70 | 71 | 1.11.8 (2014-05-21) 72 | ------------------- 73 | 74 | 1.11.7 (2014-05-21) 75 | ------------------- 76 | * uses %u instead of %d for unsigned int 77 | * Contributors: enriquefernandez 78 | 79 | 1.11.5 (2014-01-30) 80 | ------------------- 81 | * Misc. other commits from Groovy Branch 82 | * check for CATKIN_ENABLE_TESTING 83 | * Change maintainer from Hersh to Lu 84 | 85 | 1.11.4 (2013-09-27) 86 | ------------------- 87 | * Package URL Updates 88 | -------------------------------------------------------------------------------- /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/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 | -------------------------------------------------------------------------------- /carrot_planner/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package carrot_planner 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.16.2 (2018-07-31) 6 | ------------------- 7 | * Merge pull request `#773 `_ from ros-planning/packaging_fixes 8 | packaging fixes 9 | * fix depends of carrot_planner 10 | * declare direct dependency on tf2 11 | * alphabetize the depends in CMake 12 | * Contributors: Michael Ferguson 13 | 14 | 1.16.1 (2018-07-28) 15 | ------------------- 16 | 17 | 1.16.0 (2018-07-25) 18 | ------------------- 19 | * Switch to TF2 `#755 `_ 20 | * Contributors: Michael Ferguson, Rein Appeldoorn, Vincent Rabaud 21 | 22 | 1.15.2 (2018-03-22) 23 | ------------------- 24 | * Merge pull request `#673 `_ from ros-planning/email_update_lunar 25 | update maintainer email (lunar) 26 | * Merge pull request `#649 `_ from aaronhoy/lunar_add_ahoy 27 | Add myself as a maintainer. 28 | * Contributors: Aaron Hoy, Michael Ferguson 29 | 30 | 1.15.1 (2017-08-14) 31 | ------------------- 32 | 33 | 1.15.0 (2017-08-07) 34 | ------------------- 35 | * convert packages to format2 36 | * Fix CMakeLists + package.xmls (`#548 `_) 37 | * Contributors: Martin Günther, Mikael Arguedas, Vincent Rabaud 38 | 39 | 1.14.0 (2016-05-20) 40 | ------------------- 41 | 42 | 1.13.1 (2015-10-29) 43 | ------------------- 44 | 45 | 1.13.0 (2015-03-17) 46 | ------------------- 47 | 48 | 1.12.0 (2015-02-04) 49 | ------------------- 50 | * update maintainer email 51 | * Contributors: Michael Ferguson 52 | 53 | 1.11.15 (2015-02-03) 54 | -------------------- 55 | * Add ARCHIVE_DESTINATION for static builds 56 | * Contributors: Gary Servin 57 | 58 | 1.11.14 (2014-12-05) 59 | -------------------- 60 | 61 | 1.11.13 (2014-10-02) 62 | -------------------- 63 | 64 | 1.11.12 (2014-10-01) 65 | -------------------- 66 | 67 | 1.11.11 (2014-07-23) 68 | -------------------- 69 | 70 | 1.11.10 (2014-06-25) 71 | -------------------- 72 | 73 | 1.11.9 (2014-06-10) 74 | ------------------- 75 | 76 | 1.11.8 (2014-05-21) 77 | ------------------- 78 | 79 | 1.11.7 (2014-05-21) 80 | ------------------- 81 | 82 | 1.11.4 (2013-09-27) 83 | ------------------- 84 | * Package URL Updates 85 | -------------------------------------------------------------------------------- /base_local_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | base_local_planner 5 | 1.16.2 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 | -------------------------------------------------------------------------------- /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/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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /nav_core/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package nav_core 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.16.2 (2018-07-31) 6 | ------------------- 7 | 8 | 1.16.1 (2018-07-28) 9 | ------------------- 10 | 11 | 1.16.0 (2018-07-25) 12 | ------------------- 13 | * Switch to TF2 `#755 `_ 14 | * unify parameter names between base_local_planner and dwa_local_planner 15 | addresses parts of `#90 `_ 16 | * fix param names of RotateRecovery, closes `#706 `_ 17 | * Contributors: David V. Lu, Michael Ferguson, Vincent Rabaud 18 | 19 | 1.15.2 (2018-03-22) 20 | ------------------- 21 | * Merge pull request `#673 `_ from ros-planning/email_update_lunar 22 | update maintainer email (lunar) 23 | * Merge pull request `#649 `_ from aaronhoy/lunar_add_ahoy 24 | Add myself as a maintainer. 25 | * Contributors: Aaron Hoy, Michael Ferguson 26 | 27 | 1.15.1 (2017-08-14) 28 | ------------------- 29 | 30 | 1.15.0 (2017-08-07) 31 | ------------------- 32 | * convert packages to format2 33 | * makePlan overload must return. 34 | * Contributors: Eric Tappan, Mikael Arguedas 35 | 36 | 1.14.0 (2016-05-20) 37 | ------------------- 38 | 39 | 1.13.1 (2015-10-29) 40 | ------------------- 41 | * Merge pull request `#338 `_ from mikeferguson/planner_review 42 | * fix doxygen, couple style fixes 43 | * Contributors: Michael Ferguson 44 | 45 | 1.13.0 (2015-03-17) 46 | ------------------- 47 | * adding makePlan function with returned cost to base_global_planner 48 | * Contributors: phil0stine 49 | 50 | 1.12.0 (2015-02-04) 51 | ------------------- 52 | * update maintainer email 53 | * Contributors: Michael Ferguson 54 | 55 | 1.11.15 (2015-02-03) 56 | -------------------- 57 | 58 | 1.11.14 (2014-12-05) 59 | -------------------- 60 | 61 | 1.11.13 (2014-10-02) 62 | -------------------- 63 | 64 | 1.11.12 (2014-10-01) 65 | -------------------- 66 | 67 | 1.11.11 (2014-07-23) 68 | -------------------- 69 | 70 | 1.11.10 (2014-06-25) 71 | -------------------- 72 | 73 | 1.11.9 (2014-06-10) 74 | ------------------- 75 | 76 | 1.11.8 (2014-05-21) 77 | ------------------- 78 | 79 | 1.11.7 (2014-05-21) 80 | ------------------- 81 | 82 | 1.11.4 (2013-09-27) 83 | ------------------- 84 | * Package URL Updates 85 | -------------------------------------------------------------------------------- /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 | bool getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector >& path); 49 | }; 50 | 51 | } //end namespace global_planner 52 | #endif 53 | -------------------------------------------------------------------------------- /amcl/test/small_loop_prf.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 46 | 47 | -------------------------------------------------------------------------------- /costmap_2d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | costmap_2d 5 | 1.16.2 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 | geometry_msgs 34 | laser_geometry 35 | map_msgs 36 | message_filters 37 | nav_msgs 38 | pluginlib 39 | roscpp 40 | rostest 41 | sensor_msgs 42 | std_msgs 43 | tf2 44 | tf2_ros 45 | visualization_msgs 46 | voxel_grid 47 | 48 | message_runtime 49 | rosconsole 50 | map_server 51 | rosbag 52 | rostest 53 | rosunit 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /amcl/test/global_localization_stage.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 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 | 49 | 50 | -------------------------------------------------------------------------------- /amcl/test/rosie_multilaser.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 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 | 49 | 50 | -------------------------------------------------------------------------------- /move_base/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | move_base 5 | 1.16.2 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 | -------------------------------------------------------------------------------- /amcl/test/basic_localization_stage.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 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 | 49 | 50 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /amcl/test/set_initial_pose.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 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 | 47 | 48 | 50 | 51 | -------------------------------------------------------------------------------- /amcl/test/small_loop_crazy_driving_prg.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 49 | 50 | -------------------------------------------------------------------------------- /amcl/test/small_loop_crazy_driving_prg_corrected.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 49 | 50 | -------------------------------------------------------------------------------- /amcl/test/texas_willow_hallway_loop.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 | 37 | 38 | 48 | 49 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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("restore_defaults", bool_t, 0, "Restore to the original configuration", False) 30 | exit(gen.generate(PACKAGE, "move_base_node", "MoveBase")) 31 | -------------------------------------------------------------------------------- /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 | 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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /map_server/test/test_constants.cpp: -------------------------------------------------------------------------------- 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 | 30 | /* Author: Brian Gerkey */ 31 | 32 | /* This file contains global constants shared among tests */ 33 | 34 | /* Note that these must be changed if the test image changes */ 35 | 36 | #include "test_constants.h" 37 | 38 | const unsigned int g_valid_image_width = 10; 39 | const unsigned int g_valid_image_height = 10; 40 | // Note that the image content is given in row-major order, with the 41 | // lower-left pixel first. This is different from a graphics coordinate 42 | // system, which starts with the upper-left pixel. The loadMapFromFile 43 | // call converts from the latter to the former when it loads the image, and 44 | // we want to compare against the result of that conversion. 45 | const char g_valid_image_content[] = { 46 | 0,0,0,0,0,0,0,0,0,0, 47 | 100,100,100,100,0,0,100,100,100,0, 48 | 100,100,100,100,0,0,100,100,100,0, 49 | 100,0,0,0,0,0,0,0,0,0, 50 | 100,0,0,0,0,0,0,0,0,0, 51 | 100,0,0,0,0,0,100,100,0,0, 52 | 100,0,0,0,0,0,100,100,0,0, 53 | 100,0,0,0,0,0,100,100,0,0, 54 | 100,0,0,0,0,0,100,100,0,0, 55 | 100,0,0,0,0,0,0,0,0,0, 56 | }; 57 | 58 | const char* g_valid_png_file = "test/testmap.png"; 59 | const char* g_valid_bmp_file = "test/testmap.bmp"; 60 | 61 | const float g_valid_image_res = 0.1; 62 | 63 | -------------------------------------------------------------------------------- /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 | * Whether this generator can create more trajectories 54 | */ 55 | virtual bool hasMoreTrajectories() = 0; 56 | 57 | /** 58 | * Whether this generator can create more trajectories 59 | */ 60 | virtual bool nextTrajectory(Trajectory &traj) = 0; 61 | 62 | /** 63 | * @brief Virtual destructor for the interface 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/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/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 Interface for modules finding a trajectory to use for navigation commands next 48 | */ 49 | class TrajectorySearch { 50 | public: 51 | /** 52 | * searches the space of allowed trajectory and 53 | * returns one considered the optimal given the 54 | * constraints of the particular search. 55 | * 56 | * @param traj The container to write the result to 57 | * @param all_explored pass NULL or a container to collect all trajectories for debugging (has a penalty) 58 | */ 59 | virtual bool findBestTrajectory(Trajectory& traj, std::vector* all_explored) = 0; 60 | 61 | virtual ~TrajectorySearch() {} 62 | 63 | protected: 64 | TrajectorySearch() {} 65 | 66 | }; 67 | 68 | 69 | } 70 | 71 | #endif /* TRAJECTORY_SEARCH_H_ */ 72 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | 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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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/test/set_initial_pose_delayed.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 8 | 9 | 10 | 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 | 51 | 52 | 54 | 55 | -------------------------------------------------------------------------------- /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 | bool calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y, int cycles, 67 | float* potential); 68 | private: 69 | void add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x, int end_y); 70 | std::vector queue_; 71 | }; 72 | 73 | } //end namespace global_planner 74 | #endif 75 | 76 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | --------------------------------------------------------------------------------