├── README.md ├── costmap_queue ├── CMakeLists.txt ├── README.md ├── include │ └── costmap_queue │ │ ├── costmap_queue.h │ │ ├── limited_costmap_queue.h │ │ └── map_based_queue.h ├── package.xml ├── src │ ├── costmap_queue.cpp │ └── limited_costmap_queue.cpp └── test │ ├── mbq_test.cpp │ └── utest.cpp ├── dlux_global_planner ├── CMakeLists.txt ├── Derivation.md ├── README.md ├── include │ └── dlux_global_planner │ │ ├── cost_interpreter.h │ │ ├── dlux_global_planner.h │ │ ├── kernel_function.h │ │ ├── potential.h │ │ ├── potential_calculator.h │ │ └── traceback.h ├── nav_core2_plugins.xml ├── package.xml ├── src │ ├── cost_interpreter.cpp │ ├── dlux_global_planner.cpp │ └── planner_node.cpp └── test │ └── kernel_test.cpp ├── dlux_plugins ├── CMakeLists.txt ├── README.md ├── dlux_global_planner_plugins.xml ├── doc │ ├── 0-AllPaths.png │ ├── 10-Dij.png │ ├── 11-AMan.png │ ├── 12-ACart.png │ ├── 13-AManKer.png │ ├── 14-ACartKer.png │ ├── 20-Von.png │ ├── 21-Grid.png │ ├── 22-Grad.png │ ├── 30-Optimal.png │ └── PlanLengthVsCellsExpanded.png ├── include │ └── dlux_plugins │ │ ├── astar.h │ │ ├── dijkstra.h │ │ ├── gradient_path.h │ │ ├── grid_path.h │ │ └── von_neumann_path.h ├── package.xml ├── src │ ├── astar.cpp │ ├── dijkstra.cpp │ ├── gradient_path.cpp │ ├── grid_path.cpp │ └── von_neumann_path.cpp └── test │ ├── common.yaml │ ├── full_planner_test.cpp │ ├── full_planner_test.launch │ ├── global_oscillation_test.cpp │ ├── global_oscillation_test.launch │ ├── map.png │ ├── map.yaml │ ├── node_test.launch │ ├── planner_test.cpp │ ├── planner_test.launch │ ├── robert_frost.png │ └── rviz.rviz ├── dwb_critics ├── CMakeLists.txt ├── README.md ├── default_critics.xml ├── include │ └── dwb_critics │ │ ├── alignment_util.h │ │ ├── base_obstacle.h │ │ ├── goal_align.h │ │ ├── goal_dist.h │ │ ├── map_grid.h │ │ ├── obstacle_footprint.h │ │ ├── oscillation.h │ │ ├── path_align.h │ │ ├── path_dist.h │ │ ├── prefer_forward.h │ │ ├── rotate_to_goal.h │ │ └── twirling.h ├── package.xml └── src │ ├── alignment_util.cpp │ ├── base_obstacle.cpp │ ├── goal_align.cpp │ ├── goal_dist.cpp │ ├── map_grid.cpp │ ├── obstacle_footprint.cpp │ ├── oscillation.cpp │ ├── path_align.cpp │ ├── path_dist.cpp │ ├── prefer_forward.cpp │ ├── rotate_to_goal.cpp │ └── twirling.cpp ├── dwb_local_planner ├── CMakeLists.txt ├── README.md ├── include │ └── dwb_local_planner │ │ ├── backwards_compatibility.h │ │ ├── debug_dwb_local_planner.h │ │ ├── dwb_local_planner.h │ │ ├── goal_checker.h │ │ ├── illegal_trajectory_tracker.h │ │ ├── publisher.h │ │ ├── trajectory_critic.h │ │ ├── trajectory_generator.h │ │ └── trajectory_utils.h ├── launch │ └── plan_node.launch ├── package.xml ├── plugins.xml ├── src │ ├── backwards_compatibility.cpp │ ├── debug_dwb_local_planner.cpp │ ├── dwb_local_planner.cpp │ ├── illegal_trajectory_tracker.cpp │ ├── planner_node.cpp │ ├── publisher.cpp │ └── trajectory_utils.cpp └── test │ └── utils_test.cpp ├── dwb_msgs ├── CMakeLists.txt ├── msg │ ├── CriticScore.msg │ ├── LocalPlanEvaluation.msg │ ├── Trajectory2D.msg │ └── TrajectoryScore.msg ├── package.xml └── srv │ ├── DebugLocalPlan.srv │ ├── GenerateTrajectory.srv │ ├── GenerateTwists.srv │ ├── GetCriticScore.srv │ └── ScoreTrajectory.srv ├── dwb_plugins ├── CMakeLists.txt ├── README.md ├── cfg │ └── KinematicParams.cfg ├── doc │ ├── VelocitySpace.png │ ├── lim_pv.png │ └── std_pv.png ├── include │ └── dwb_plugins │ │ ├── kinematic_parameters.h │ │ ├── limited_accel_generator.h │ │ ├── one_d_velocity_iterator.h │ │ ├── simple_goal_checker.h │ │ ├── standard_traj_generator.h │ │ ├── stopped_goal_checker.h │ │ ├── velocity_iterator.h │ │ └── xy_theta_iterator.h ├── package.xml ├── plugins.xml ├── src │ ├── kinematic_parameters.cpp │ ├── limited_accel_generator.cpp │ ├── simple_goal_checker.cpp │ ├── standard_traj_generator.cpp │ ├── stopped_goal_checker.cpp │ └── xy_theta_iterator.cpp └── test │ ├── goal_checker.cpp │ ├── goal_checker.launch │ ├── twist_gen.cpp │ ├── twist_gen.launch │ └── velocity_iterator_test.cpp ├── global_planner_tests ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── config │ └── standard_tests.yaml ├── include │ └── global_planner_tests │ │ ├── easy_costmap.h │ │ ├── global_planner_tests.h │ │ ├── many_map_test_suite.h │ │ └── util.h ├── maps │ ├── corner.png │ ├── empty.png │ ├── inflated.png │ ├── nopaths.png │ ├── smallloop.png │ ├── smile.png │ └── unknown.png ├── package.xml └── src │ ├── easy_costmap.cpp │ ├── global_planner_tests.cpp │ ├── gpt_node.cpp │ ├── heatmap_node.cpp │ ├── many_map_node.cpp │ ├── many_map_test_suite.cpp │ └── util.cpp ├── locomotor ├── CMakeLists.txt ├── README.md ├── doc │ ├── BuildingOnTop.md │ ├── ErrorHandling.md │ ├── ExampleStateMachines.md │ ├── Motivation.md │ ├── MultiplePlanners.md │ ├── PrimaryDesign.md │ └── figures │ │ ├── action_server.flow │ │ ├── action_server.png │ │ ├── error_handling.flow │ │ ├── error_handling.png │ │ ├── generate.py │ │ ├── single_thread.flow │ │ ├── single_thread.png │ │ ├── two_thread.flow │ │ └── two_thread.png ├── include │ └── locomotor │ │ ├── executor.h │ │ ├── locomotor.h │ │ ├── locomotor_action_server.h │ │ └── publishers.h ├── package.xml ├── scripts │ └── send_action.py └── src │ ├── double_thread_locomotor.cpp │ ├── executor.cpp │ ├── locomotor.cpp │ ├── locomotor_action_server.cpp │ ├── publishers.cpp │ └── single_thread_locomotor.cpp ├── locomotor_msgs ├── CMakeLists.txt ├── README.md ├── action │ └── NavigateToPose.action ├── msg │ ├── NavigationState.msg │ └── ResultCode.msg └── package.xml ├── locomove_base ├── CMakeLists.txt ├── README.md ├── doc │ └── state_machine.png ├── include │ └── locomove_base │ │ └── locomove_base.h ├── package.xml └── src │ └── locomove_base.cpp ├── nav_2d_msgs ├── CMakeLists.txt ├── README.md ├── msg │ ├── ComplexPolygon2D.msg │ ├── NavGridInfo.msg │ ├── NavGridOfChars.msg │ ├── NavGridOfCharsUpdate.msg │ ├── NavGridOfDoubles.msg │ ├── NavGridOfDoublesUpdate.msg │ ├── Path2D.msg │ ├── Point2D.msg │ ├── Polygon2D.msg │ ├── Polygon2DCollection.msg │ ├── Polygon2DStamped.msg │ ├── Pose2D32.msg │ ├── Pose2DStamped.msg │ ├── Twist2D.msg │ ├── Twist2D32.msg │ ├── Twist2DStamped.msg │ └── UIntBounds.msg ├── package.xml └── srv │ └── SwitchPlugin.srv ├── nav_2d_utils ├── CMakeLists.txt ├── README.md ├── doc │ ├── Conversions.md │ ├── PluginMux.md │ └── PolygonsAndFootprints.md ├── include │ ├── mapbox │ │ ├── LICENSE │ │ ├── NOTES │ │ └── earcut.hpp │ └── nav_2d_utils │ │ ├── bounds.h │ │ ├── conversions.h │ │ ├── footprint.h │ │ ├── geometry_help.h │ │ ├── odom_subscriber.h │ │ ├── parameters.h │ │ ├── path_ops.h │ │ ├── plugin_mux.h │ │ ├── polygons.h │ │ └── tf_help.h ├── package.xml ├── setup.py ├── src │ ├── bounds.cpp │ ├── conversions.cpp │ ├── footprint.cpp │ ├── nav_2d_utils │ │ ├── __init__.py │ │ └── conversions.py │ ├── path_ops.cpp │ ├── polygons.cpp │ └── tf_help.cpp └── test │ ├── bounds_test.cpp │ ├── compress_test.cpp │ ├── param_tests.cpp │ ├── param_tests.launch │ ├── polygon_tests.cpp │ └── resolution_test.cpp ├── nav_core2 ├── CMakeLists.txt ├── README.md ├── doc │ └── exceptions.png ├── include │ └── nav_core2 │ │ ├── basic_costmap.h │ │ ├── bounds.h │ │ ├── common.h │ │ ├── costmap.h │ │ ├── exceptions.h │ │ ├── global_planner.h │ │ └── local_planner.h ├── package.xml ├── src │ └── basic_costmap.cpp └── test │ ├── bounds_test.cpp │ └── exception_test.cpp ├── nav_core_adapter ├── CMakeLists.txt ├── README.md ├── include │ └── nav_core_adapter │ │ ├── costmap_adapter.h │ │ ├── global_planner_adapter.h │ │ ├── global_planner_adapter2.h │ │ ├── local_planner_adapter.h │ │ └── shared_pointers.h ├── nav_core2_plugins.xml ├── nav_core_plugins.xml ├── package.xml ├── src │ ├── costmap_adapter.cpp │ ├── global_planner_adapter.cpp │ ├── global_planner_adapter2.cpp │ └── local_planner_adapter.cpp └── test │ ├── unload_test.cpp │ └── unload_test.launch ├── nav_grid ├── CMakeLists.txt ├── README.md ├── doc │ ├── change_info.png │ ├── coords.png │ └── nav_grid.png ├── include │ └── nav_grid │ │ ├── coordinate_conversion.h │ │ ├── index.h │ │ ├── nav_grid.h │ │ ├── nav_grid_info.h │ │ └── vector_nav_grid.h ├── package.xml └── test │ └── utest.cpp ├── nav_grid_iterators ├── CMakeLists.txt ├── README.md ├── demo │ ├── demo.cpp │ ├── demo.launch │ ├── demo.mp4 │ └── demo.rviz ├── include │ └── nav_grid_iterators │ │ ├── base_iterator.h │ │ ├── circle_fill.h │ │ ├── circle_outline.h │ │ ├── iterators.h │ │ ├── line.h │ │ ├── line │ │ ├── abstract_line_iterator.h │ │ ├── bresenham.h │ │ └── ray_trace.h │ │ ├── polygon_fill.h │ │ ├── polygon_outline.h │ │ ├── spiral.h │ │ ├── sub_grid.h │ │ └── whole_grid.h ├── package.xml ├── src │ ├── bresenham.cpp │ ├── circle_fill.cpp │ ├── circle_outline.cpp │ ├── line.cpp │ ├── polygon_fill.cpp │ ├── polygon_outline.cpp │ ├── ray_trace.cpp │ ├── spiral.cpp │ ├── sub_grid.cpp │ └── whole_grid.cpp └── test │ ├── line_tests.cpp │ └── utest.cpp ├── nav_grid_pub_sub ├── CMakeLists.txt ├── README.md ├── doc │ ├── CostInterpretation.md │ ├── input0.png │ ├── input1.png │ ├── negate.png │ ├── publish.png │ ├── raw.png │ ├── scale_output.png │ ├── scale_pixel.png │ ├── trinary_input.png │ ├── trinary_output.png │ └── trinary_pixel.png ├── include │ └── nav_grid_pub_sub │ │ ├── cost_interpretation.h │ │ ├── cost_interpretation_tables.h │ │ ├── nav_grid_message_utils.h │ │ ├── nav_grid_publisher.h │ │ ├── nav_grid_subscriber.h │ │ └── occ_grid_message_utils.h ├── package.xml └── src │ └── cost_interpretation_tables.cpp ├── nav_grid_server ├── CMakeLists.txt ├── README.md ├── include │ └── nav_grid_server │ │ └── image_loader.h ├── package.xml ├── src │ ├── image_loader.cpp │ ├── saver.cpp │ └── server.cpp └── test │ ├── data │ ├── alpha.png │ ├── map.yaml │ ├── spectrum.pgm │ └── spectrum.png │ └── map_server_comparison.cpp ├── robot_nav_tools ├── color_util │ ├── CMakeLists.txt │ ├── README.md │ ├── doc │ │ ├── blend00.png │ │ ├── blend01.png │ │ ├── blend02.png │ │ └── named.png │ ├── include │ │ └── color_util │ │ │ ├── blend.h │ │ │ ├── convert.h │ │ │ ├── named_colors.h │ │ │ └── types.h │ ├── package.xml │ ├── src │ │ └── convert.cpp │ └── test │ │ └── utest.cpp ├── robot_nav_rviz_plugins │ ├── CMakeLists.txt │ ├── README.md │ ├── icons │ │ └── classes │ │ │ ├── NavGridOfChars.png │ │ │ ├── NavGridOfDoubles.png │ │ │ ├── OccupancyGrid.png │ │ │ ├── Path2D.png │ │ │ ├── Polygon2D.png │ │ │ ├── Polygon3D.png │ │ │ └── Polygons.png │ ├── include │ │ └── robot_nav_rviz_plugins │ │ │ ├── nav_grid_display.h │ │ │ ├── nav_grid_palette.h │ │ │ ├── ogre_panel.h │ │ │ ├── path_display.h │ │ │ ├── polygon3d_display.h │ │ │ ├── polygon_display.h │ │ │ ├── polygon_parts.h │ │ │ ├── polygons_display.h │ │ │ ├── spectrum_palette.h │ │ │ └── validate_floats.h │ ├── package.xml │ ├── robot_nav_rviz_plugins.xml │ ├── rviz_plugins.xml │ └── src │ │ ├── basic_palettes.cpp │ │ ├── nav_grid_display.cpp │ │ ├── nav_grid_of_chars_display.cpp │ │ ├── nav_grid_of_doubles_display.cpp │ │ ├── occupancy_grid_display.cpp │ │ ├── ogre_panel.cpp │ │ ├── partial_ogre_panel.cpp │ │ ├── path_display.cpp │ │ ├── polygon3d_display.cpp │ │ ├── polygon_display.cpp │ │ ├── polygon_parts.cpp │ │ ├── polygons_display.cpp │ │ └── spectrum_palette.cpp ├── robot_nav_tools │ ├── CMakeLists.txt │ └── package.xml ├── robot_nav_viz_demos │ ├── CMakeLists.txt │ ├── README.md │ ├── data │ │ ├── demo_grids.bag │ │ ├── demo_grids.rviz │ │ ├── demo_polygons.rviz │ │ ├── named_colors_demo.rviz │ │ └── spectrum.rviz │ ├── include │ │ └── robot_nav_viz_demos │ │ │ └── spectrum_display.h │ ├── launch │ │ ├── grid_demo.launch │ │ ├── named_colors_demo.launch │ │ ├── polygons.launch │ │ └── spectrum_demo.launch │ ├── package.xml │ ├── robot_nav_rviz_plugins.xml │ ├── rviz_plugins.xml │ └── src │ │ ├── demo_palettes.cpp │ │ ├── named_colors_demo.cpp │ │ ├── polygon_display.cpp │ │ ├── pong.cpp │ │ └── spectrum_display.cpp └── rqt_dwb_plugin │ ├── CMakeLists.txt │ ├── README.md │ ├── doc │ ├── thumbnail.png │ ├── timeline.png │ ├── widget0.png │ └── widget1.png │ ├── package.xml │ ├── plugins.xml │ ├── scripts │ └── live_panel │ ├── setup.py │ └── src │ └── rqt_dwb_plugin │ ├── __init__.py │ ├── bounds.py │ ├── dwb_plugin.py │ ├── dwb_widget.py │ ├── eval_message_view.py │ ├── evaluation_cache.py │ ├── multi_topic_view.py │ ├── score_widget.py │ ├── sorted_twists_widget.py │ ├── table_widget.py │ ├── timeline_plotter.py │ ├── trajectory_widget.py │ ├── util.py │ └── velocity_space_widget.py └── robot_navigation ├── CMakeLists.txt └── package.xml /costmap_queue/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(costmap_queue) 3 | 4 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror") 5 | if(NOT "${CMAKE_CXX_STANDARD}") 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | nav_core2 11 | roscpp 12 | ) 13 | 14 | catkin_package( 15 | INCLUDE_DIRS include 16 | CATKIN_DEPENDS nav_core2 roscpp 17 | LIBRARIES ${PROJECT_NAME} 18 | ) 19 | 20 | include_directories( 21 | include ${catkin_INCLUDE_DIRS} 22 | ) 23 | 24 | add_library(${PROJECT_NAME} 25 | src/costmap_queue.cpp 26 | src/limited_costmap_queue.cpp 27 | ) 28 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 29 | 30 | if (CATKIN_ENABLE_TESTING) 31 | catkin_add_gtest(mbq_test test/mbq_test.cpp) 32 | target_link_libraries(mbq_test ${catkin_LIBRARIES}) 33 | 34 | catkin_add_gtest(${PROJECT_NAME}_utest test/utest.cpp) 35 | target_link_libraries(${PROJECT_NAME}_utest ${PROJECT_NAME} ${catkin_LIBRARIES}) 36 | 37 | find_package(roslint REQUIRED) 38 | roslint_cpp() 39 | roslint_add_test() 40 | endif (CATKIN_ENABLE_TESTING) 41 | 42 | install(TARGETS ${PROJECT_NAME} 43 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 44 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 45 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 46 | ) 47 | install(DIRECTORY include/${PROJECT_NAME}/ 48 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 49 | ) 50 | -------------------------------------------------------------------------------- /costmap_queue/README.md: -------------------------------------------------------------------------------- 1 | # costmap_queue 2 | 3 | This package contains helper classes that implement one particular common costmap operation: figuring out the distance of every cell to a subset of those cells. 4 | 5 | ## Inflation Example 6 | Let's assume you have a grid `G` and you know that there are lethal obstacles in a subset of those cells `O`. You want to mark all cells within distance `r` with one value, and all cells within a greater distance `r2` with another. 7 | 8 | One costly way to do this would be to to iterate through all the cells `g` in `G`, calculate the distance from `g` to each cell in `O`, find the minimum distance, and compare to `r` and `r2`. This is roughly quadratic with the number of cells. 9 | 10 | The more efficient way to do it is to start with all the cells in `O` in a queue. For each cell you dequeue, you calculate the distance to its origin cell from `O` and then enqueue all of its neighbors. This approach is roughly linear with the number of cells, although you need to be careful to not incur too many costs from resorting the queue. 11 | 12 | ## Map Based Queue 13 | While this operation could be done with the standard priority queue implementation, it can be optimized by using a custom priority queue based on `std::map`. This relies on the fact that many of the items inserted into the queue will have the same priority. In the `costmap_queue`, the priorities used are distances on the grid, of which there are a finite number. Thus by grouping all of the elements of the same weight into a vector together, and storing them in the `std::map` with their priority, we can eliminate the need to resort after each individual item is popped. This is based on the [optimizations to the Inflation algorithm](https://github.com/ros-planning/navigation/pull/525). 14 | 15 | Also, since the same (or similar) distances will be used in the `costmap_queue` from iteration to iteration, additional time can be saved by not destroying the vectors in the map, and reusing them iteration to iteration. 16 | 17 | ## Costmap Queue 18 | The `CostmapQueue` class operates on a `nav_core2::Costmap`. First, you must enqueue all of the "subset" of cells (i.e. `O` in the above example) using `enqueueCell`. Then, while the queue is not empty (i.e. `isEmpty` is false), you can call `costmap_queue::CellData cell = q.getNextCell();` to get the next cell in the queue. 19 | 20 | The `CellData` class contains 5 values: `x_` and `y_` are the coordinates for the current cell, `src_x_` and `src_y_` are the coordinates for the original cell, and `distance_` is the distance between them. 21 | 22 | By default, `CostmapQueue` will iterate through all the cells in the `Costmap`. If you want to limit it to only cells within a certain distance, you can use `LimitedCostmapQueue` instead. 23 | -------------------------------------------------------------------------------- /costmap_queue/include/costmap_queue/limited_costmap_queue.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Locus Robotics 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 copyright holder 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 HOLDER 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 | #ifndef COSTMAP_QUEUE_LIMITED_COSTMAP_QUEUE_H 36 | #define COSTMAP_QUEUE_LIMITED_COSTMAP_QUEUE_H 37 | 38 | #include 39 | 40 | namespace costmap_queue 41 | { 42 | 43 | /** 44 | * @class LimitedCostmapQueue 45 | * @brief Extension of Costmap Queue where distances are limited to a given distance from source cells. 46 | */ 47 | class LimitedCostmapQueue : public CostmapQueue 48 | { 49 | public: 50 | /** 51 | * @brief Constructor with limit as an integer number of cells. 52 | */ 53 | LimitedCostmapQueue(nav_core2::Costmap& costmap, const int cell_distance_limit); 54 | bool validCellToQueue(const CellData& cell) override; 55 | int getMaxDistance() const override; 56 | protected: 57 | int max_distance_; 58 | }; 59 | } // namespace costmap_queue 60 | 61 | #endif // COSTMAP_QUEUE_LIMITED_COSTMAP_QUEUE_H 62 | -------------------------------------------------------------------------------- /costmap_queue/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | costmap_queue 4 | 0.3.1 5 | Tool for iterating through the cells of a costmap to find the closest distance to a subset of cells. 6 | 7 | David V. Lu!! 8 | 9 | BSD 10 | 11 | catkin 12 | nav_core2 13 | roscpp 14 | roslint 15 | rosunit 16 | 17 | 18 | -------------------------------------------------------------------------------- /costmap_queue/src/limited_costmap_queue.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Locus Robotics 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 copyright holder 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 HOLDER 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 | #include 35 | 36 | namespace costmap_queue 37 | { 38 | 39 | LimitedCostmapQueue::LimitedCostmapQueue(nav_core2::Costmap& costmap, const int distance_limit) : 40 | CostmapQueue(costmap) 41 | { 42 | max_distance_ = distance_limit; 43 | reset(); 44 | } 45 | 46 | int LimitedCostmapQueue::getMaxDistance() const 47 | { 48 | return max_distance_; 49 | } 50 | 51 | bool LimitedCostmapQueue::validCellToQueue(const CellData& cell) 52 | { 53 | return cell.distance_ <= max_distance_; 54 | } 55 | 56 | } // namespace costmap_queue 57 | -------------------------------------------------------------------------------- /dlux_global_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(dlux_global_planner) 3 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror") 4 | if(NOT "${CMAKE_CXX_STANDARD}") 5 | set(CMAKE_CXX_STANDARD 14) 6 | endif() 7 | find_package(catkin REQUIRED COMPONENTS 8 | geometry_msgs 9 | nav_2d_msgs 10 | nav_2d_utils 11 | nav_core2 12 | nav_grid 13 | nav_grid_pub_sub 14 | nav_msgs 15 | pluginlib 16 | roscpp 17 | visualization_msgs 18 | ) 19 | catkin_package( 20 | CATKIN_DEPENDS 21 | geometry_msgs 22 | nav_2d_msgs 23 | nav_2d_utils 24 | nav_core2 25 | nav_grid 26 | nav_grid_pub_sub 27 | nav_msgs 28 | pluginlib 29 | roscpp 30 | visualization_msgs 31 | INCLUDE_DIRS 32 | include 33 | LIBRARIES 34 | dgp 35 | ) 36 | 37 | add_library(dgp src/dlux_global_planner.cpp src/cost_interpreter.cpp) 38 | add_dependencies(dgp ${catkin_EXPORTED_TARGETS}) 39 | target_link_libraries(dgp ${catkin_LIBRARIES}) 40 | 41 | add_executable(${PROJECT_NAME}_planner_node src/planner_node.cpp) 42 | add_dependencies(${PROJECT_NAME}_planner_node ${catkin_EXPORTED_TARGETS}) 43 | target_link_libraries(${PROJECT_NAME}_planner_node dgp ${catkin_LIBRARIES}) 44 | set_target_properties(${PROJECT_NAME}_planner_node PROPERTIES OUTPUT_NAME planner_node PREFIX "") 45 | 46 | include_directories(include ${catkin_INCLUDE_DIRS}) 47 | 48 | if (CATKIN_ENABLE_TESTING) 49 | find_package(roslint REQUIRED) 50 | roslint_cpp() 51 | roslint_add_test() 52 | 53 | find_package(rostest REQUIRED) 54 | catkin_add_gtest(kernel_test test/kernel_test.cpp) 55 | target_link_libraries(kernel_test dgp) 56 | endif() 57 | 58 | install(TARGETS ${PROJECT_NAME}_planner_node 59 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 60 | ) 61 | install(TARGETS dgp 62 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 63 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 64 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 65 | ) 66 | install(DIRECTORY include/${PROJECT_NAME}/ 67 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 68 | ) 69 | install(FILES nav_core2_plugins.xml 70 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 71 | ) 72 | -------------------------------------------------------------------------------- /dlux_global_planner/include/dlux_global_planner/potential.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 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 copyright holder 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 HOLDER 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 | #ifndef DLUX_GLOBAL_PLANNER_POTENTIAL_H 36 | #define DLUX_GLOBAL_PLANNER_POTENTIAL_H 37 | 38 | #include 39 | #include 40 | 41 | namespace dlux_global_planner 42 | { 43 | /** 44 | * Default value for potential indicating it has not yet been calculated 45 | */ 46 | const float HIGH_POTENTIAL = std::numeric_limits::max(); 47 | 48 | using PotentialGrid = nav_grid::VectorNavGrid; 49 | } // namespace dlux_global_planner 50 | 51 | #endif // DLUX_GLOBAL_PLANNER_POTENTIAL_H 52 | -------------------------------------------------------------------------------- /dlux_global_planner/nav_core2_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Plugin-based global wavefront planner that conforms to the `nav_core2` interface 4 | 5 | 6 | -------------------------------------------------------------------------------- /dlux_global_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dlux_global_planner 4 | 0.3.1 5 | 6 | Plugin based global planner implementing the nav_core2::GlobalPlanner interface. 7 | 8 | David V. Lu!! 9 | BSD 10 | catkin 11 | geometry_msgs 12 | nav_2d_msgs 13 | nav_2d_utils 14 | nav_core2 15 | nav_grid 16 | nav_grid_pub_sub 17 | nav_msgs 18 | pluginlib 19 | roscpp 20 | visualization_msgs 21 | roslint 22 | rostest 23 | rosunit 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /dlux_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(dlux_plugins) 3 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror") 4 | if(NOT "${CMAKE_CXX_STANDARD}") 5 | set(CMAKE_CXX_STANDARD 14) 6 | endif() 7 | 8 | find_package(catkin REQUIRED 9 | COMPONENTS dlux_global_planner nav_core2 nav_grid pluginlib 10 | ) 11 | 12 | catkin_package( 13 | CATKIN_DEPENDS dlux_global_planner nav_core2 nav_grid pluginlib 14 | INCLUDE_DIRS include 15 | LIBRARIES dlux_plugins 16 | ) 17 | 18 | add_library(dlux_plugins src/dijkstra.cpp src/astar.cpp src/grid_path.cpp src/gradient_path.cpp src/von_neumann_path.cpp) 19 | target_link_libraries(dlux_plugins ${catkin_LIBRARIES}) 20 | include_directories( 21 | include ${catkin_INCLUDE_DIRS} 22 | ) 23 | 24 | if (CATKIN_ENABLE_TESTING) 25 | find_package(rostest REQUIRED) 26 | 27 | find_package(global_planner_tests REQUIRED) 28 | include_directories(include ${global_planner_tests_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${GTEST_INCLUDE_DIRS}) 29 | 30 | add_rostest_gtest(planner_test test/planner_test.launch test/planner_test.cpp) 31 | target_link_libraries(planner_test ${global_planner_tests_LIBRARIES} ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) 32 | 33 | # full_planner_test contains many parameter variations and does not need to be run regularly 34 | # add_rostest_gtest(full_planner_test test/full_planner_test.launch test/full_planner_test.cpp) 35 | # target_link_libraries(full_planner_test ${global_planner_tests_LIBRARIES} ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) 36 | 37 | add_rostest_gtest(got test/global_oscillation_test.launch test/global_oscillation_test.cpp) 38 | target_link_libraries(got ${global_planner_tests_LIBRARIES} ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) 39 | 40 | find_package(roslint REQUIRED) 41 | roslint_cpp() 42 | roslint_add_test() 43 | endif() 44 | 45 | install( 46 | TARGETS dlux_plugins 47 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 48 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 49 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 50 | ) 51 | install( 52 | DIRECTORY include/${PROJECT_NAME}/ 53 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 54 | ) 55 | install( 56 | FILES dlux_global_planner_plugins.xml 57 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 58 | ) 59 | install( 60 | DIRECTORY test 61 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 62 | ) 63 | -------------------------------------------------------------------------------- /dlux_plugins/dlux_global_planner_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Potential calculator that explores using a distance heuristic (A*) but not the kernel function 4 | 5 | 6 | Potential calculator that explores the potential breadth first while using the kernel function 7 | 8 | 9 | Traceback function that moves from cell to cell using only the four neighbors 10 | 11 | 12 | Traceback function that moves from cell to cell using any of the eight neighbors 13 | 14 | 15 | Traceback function that creates a smooth gradient from the start to the goal 16 | 17 | 18 | -------------------------------------------------------------------------------- /dlux_plugins/doc/0-AllPaths.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/dlux_plugins/doc/0-AllPaths.png -------------------------------------------------------------------------------- /dlux_plugins/doc/10-Dij.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/dlux_plugins/doc/10-Dij.png -------------------------------------------------------------------------------- /dlux_plugins/doc/11-AMan.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/dlux_plugins/doc/11-AMan.png -------------------------------------------------------------------------------- /dlux_plugins/doc/12-ACart.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/dlux_plugins/doc/12-ACart.png -------------------------------------------------------------------------------- /dlux_plugins/doc/13-AManKer.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/dlux_plugins/doc/13-AManKer.png -------------------------------------------------------------------------------- /dlux_plugins/doc/14-ACartKer.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/dlux_plugins/doc/14-ACartKer.png -------------------------------------------------------------------------------- /dlux_plugins/doc/20-Von.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/dlux_plugins/doc/20-Von.png -------------------------------------------------------------------------------- /dlux_plugins/doc/21-Grid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/dlux_plugins/doc/21-Grid.png -------------------------------------------------------------------------------- /dlux_plugins/doc/22-Grad.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/dlux_plugins/doc/22-Grad.png -------------------------------------------------------------------------------- /dlux_plugins/doc/30-Optimal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/dlux_plugins/doc/30-Optimal.png -------------------------------------------------------------------------------- /dlux_plugins/doc/PlanLengthVsCellsExpanded.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/dlux_plugins/doc/PlanLengthVsCellsExpanded.png -------------------------------------------------------------------------------- /dlux_plugins/include/dlux_plugins/grid_path.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 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 copyright holder 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 HOLDER 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 | #ifndef DLUX_PLUGINS_GRID_PATH_H 36 | #define DLUX_PLUGINS_GRID_PATH_H 37 | 38 | #include 39 | 40 | namespace dlux_plugins 41 | { 42 | /** 43 | * @class GridPath 44 | * @brief Traceback function that moves from cell to cell using any of the eight neighbors 45 | */ 46 | class GridPath : public dlux_global_planner::Traceback 47 | { 48 | public: 49 | // Main Traceback interface 50 | nav_2d_msgs::Path2D getPath(const dlux_global_planner::PotentialGrid& potential_grid, 51 | const geometry_msgs::Pose2D& start, const geometry_msgs::Pose2D& goal, 52 | double& path_cost) override; 53 | }; 54 | } // namespace dlux_plugins 55 | 56 | #endif // DLUX_PLUGINS_GRID_PATH_H 57 | -------------------------------------------------------------------------------- /dlux_plugins/include/dlux_plugins/von_neumann_path.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 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 copyright holder 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 HOLDER 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 | #ifndef DLUX_PLUGINS_VON_NEUMANN_PATH_H 36 | #define DLUX_PLUGINS_VON_NEUMANN_PATH_H 37 | 38 | #include 39 | 40 | namespace dlux_plugins 41 | { 42 | /** 43 | * @class VonNeumannPath 44 | * @brief Traceback function that moves from cell to cell using only the four neighbors 45 | */ 46 | class VonNeumannPath : public dlux_global_planner::Traceback 47 | { 48 | public: 49 | // Main Traceback interface 50 | nav_2d_msgs::Path2D getPath(const dlux_global_planner::PotentialGrid& potential_grid, 51 | const geometry_msgs::Pose2D& start, const geometry_msgs::Pose2D& goal, 52 | double& path_cost) override; 53 | }; 54 | } // namespace dlux_plugins 55 | 56 | #endif // DLUX_PLUGINS_VON_NEUMANN_PATH_H 57 | -------------------------------------------------------------------------------- /dlux_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dlux_plugins 4 | 0.3.1 5 | 6 | Implementation of dlux_global_planner plugin interfaces. 7 | 8 | David V. Lu!! 9 | BSD 10 | 11 | catkin 12 | dlux_global_planner 13 | nav_core2 14 | nav_grid 15 | pluginlib 16 | global_planner_tests 17 | rostest 18 | roslint 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /dlux_plugins/test/common.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | plugins: 3 | - {name: static_layer, type: 'costmap_2d::StaticLayer'} 4 | static_layer: 5 | trinary_costmap: false 6 | planner: 7 | publish_potential: true 8 | print_statistics: true 9 | -------------------------------------------------------------------------------- /dlux_plugins/test/full_planner_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /dlux_plugins/test/global_oscillation_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /dlux_plugins/test/map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/dlux_plugins/test/map.png -------------------------------------------------------------------------------- /dlux_plugins/test/map.yaml: -------------------------------------------------------------------------------- 1 | image: map.png 2 | resolution: 0.1 3 | origin: [0, 0, 0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | mode: scale 8 | -------------------------------------------------------------------------------- /dlux_plugins/test/planner_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /dlux_plugins/test/robert_frost.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/dlux_plugins/test/robert_frost.png -------------------------------------------------------------------------------- /dlux_plugins/test/rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Name: Displays 4 | Visualization Manager: 5 | Displays: 6 | - Alpha: 1 7 | Class: rviz/Map 8 | Enabled: true 9 | Name: Static Map 10 | Topic: /map 11 | Value: true 12 | - Class: rviz/Marker 13 | Enabled: true 14 | Marker Topic: /visualization_marker 15 | Name: Markers 16 | Value: true 17 | - Alpha: 0.5 18 | Class: rviz/Map 19 | Color Scheme: map 20 | Enabled: false 21 | Name: Potential Map 22 | Topic: /astar_grad/planner/potential 23 | Value: false 24 | Enabled: true 25 | Global Options: 26 | Background Color: 48; 48; 48 27 | Fixed Frame: map 28 | Frame Rate: 30 29 | Name: root 30 | Tools: 31 | - Class: rviz/MoveCamera 32 | - Class: rviz/SetInitialPose 33 | Topic: /initialpose 34 | - Class: rviz/SetGoal 35 | Topic: /move_base_simple/goal 36 | Value: true 37 | Views: 38 | Current: 39 | Angle: 0 40 | Class: rviz/TopDownOrtho 41 | Name: Current View 42 | Scale: 30 43 | Value: TopDownOrtho (rviz) 44 | X: 7.5 45 | Y: 7.5 46 | -------------------------------------------------------------------------------- /dwb_critics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(dwb_critics) 3 | 4 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror") 5 | if(NOT "${CMAKE_CXX_STANDARD}") 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | angles 11 | costmap_queue 12 | dwb_local_planner 13 | geometry_msgs 14 | nav_2d_msgs 15 | nav_2d_utils 16 | nav_core2 17 | nav_grid_iterators 18 | pluginlib 19 | roscpp 20 | sensor_msgs 21 | ) 22 | 23 | catkin_package( 24 | INCLUDE_DIRS include 25 | LIBRARIES ${PROJECT_NAME} 26 | CATKIN_DEPENDS angles costmap_queue dwb_local_planner geometry_msgs nav_2d_msgs nav_2d_utils nav_core2 nav_grid_iterators pluginlib roscpp sensor_msgs 27 | ) 28 | 29 | include_directories( 30 | include 31 | ${catkin_INCLUDE_DIRS} 32 | ) 33 | 34 | add_library(${PROJECT_NAME} 35 | src/alignment_util.cpp 36 | src/map_grid.cpp 37 | src/goal_dist.cpp 38 | src/path_dist.cpp 39 | src/goal_align.cpp 40 | src/path_align.cpp 41 | src/base_obstacle.cpp 42 | src/obstacle_footprint.cpp 43 | src/oscillation.cpp 44 | src/prefer_forward.cpp 45 | src/rotate_to_goal.cpp 46 | src/twirling.cpp 47 | ) 48 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 49 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 50 | 51 | if(CATKIN_ENABLE_TESTING) 52 | find_package(roslint REQUIRED) 53 | roslint_cpp() 54 | roslint_add_test() 55 | endif() 56 | 57 | install(TARGETS ${PROJECT_NAME} 58 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 59 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 60 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 61 | ) 62 | install(DIRECTORY include/${PROJECT_NAME}/ 63 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 64 | ) 65 | install(FILES default_critics.xml 66 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 67 | ) 68 | -------------------------------------------------------------------------------- /dwb_critics/default_critics.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Penalize trajectories with move backwards and/or turn too much 5 | 6 | 7 | Scores trajectories based on how far along the global path they end up. 8 | 9 | 10 | Scores trajectories based on how far from the global path the front of the robot ends up. 11 | 12 | 13 | 14 | Scores trajectories based on whether the robot ends up pointing toward the eventual goal 15 | 16 | 17 | 18 | Scores trajectories based on how far from the global path they end up. 19 | 20 | 21 | Checks to see whether the sign of the commanded velocity flips frequently 22 | 23 | 24 | Forces the commanded trajectories to only be rotations if within a certain distance window 25 | 26 | 27 | 28 | Uses costmap 2d to assign negative costs if a circular robot 29 | would collide at any point of the trajectory. 30 | 31 | 32 | 33 | Uses costmap 2d to assign negative costs if robot footprint is in obstacle 34 | on any point of the trajectory. 35 | 36 | 37 | 38 | Penalize trajectories with rotational velocities 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /dwb_critics/include/dwb_critics/alignment_util.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Locus Robotics 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 copyright holder 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 HOLDER 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 | #ifndef DWB_CRITICS_ALIGNMENT_UTIL_H 36 | #define DWB_CRITICS_ALIGNMENT_UTIL_H 37 | 38 | #include 39 | 40 | namespace dwb_critics 41 | { 42 | /** 43 | * @brief Projects the given pose forward the specified distance in the x direction. 44 | * @param pose Input pose 45 | * @param distance distance to move (in meters) 46 | * @return Pose distance meters in front of input pose. 47 | * 48 | * (used in both path_align and dist_align) 49 | */ 50 | geometry_msgs::Pose2D getForwardPose(const geometry_msgs::Pose2D& pose, double distance); 51 | 52 | } // namespace dwb_critics 53 | 54 | #endif // DWB_CRITICS_ALIGNMENT_UTIL_H 55 | -------------------------------------------------------------------------------- /dwb_critics/include/dwb_critics/path_dist.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Locus Robotics 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 copyright holder 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 HOLDER 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 DWB_CRITICS_PATH_DIST_H_ 35 | #define DWB_CRITICS_PATH_DIST_H_ 36 | 37 | #include 38 | 39 | namespace dwb_critics 40 | { 41 | /** 42 | * @class PathDistCritic 43 | * @brief Scores trajectories based on how far from the global path they end up. 44 | */ 45 | class PathDistCritic: public MapGridCritic 46 | { 47 | public: 48 | bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel, 49 | const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan) override; 50 | }; 51 | 52 | } // namespace dwb_critics 53 | #endif // DWB_CRITICS_PATH_DIST_H_ 54 | -------------------------------------------------------------------------------- /dwb_critics/include/dwb_critics/twirling.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Locus Robotics 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 copyright holder 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 HOLDER 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 | #ifndef DWB_CRITICS_TWIRLING_H 36 | #define DWB_CRITICS_TWIRLING_H 37 | 38 | #include 39 | 40 | namespace dwb_critics 41 | { 42 | /** 43 | * @class TwirlingCritic 44 | * @brief Penalize trajectories with rotational velocities 45 | * 46 | * This class provides a cost based on how much a robot "twirls" on its way to the goal. With 47 | * differential-drive robots, there isn't a choice, but with holonomic or near-holonomic robots, 48 | * sometimes a robot spins more than you'd like on its way to a goal. This class provides a way 49 | * to assign a penalty purely to rotational velocities. 50 | */ 51 | class TwirlingCritic: public dwb_local_planner::TrajectoryCritic 52 | { 53 | public: 54 | void onInit() override; 55 | double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override; 56 | }; 57 | } // namespace dwb_critics 58 | 59 | #endif // DWB_CRITICS_TWIRLING_H 60 | -------------------------------------------------------------------------------- /dwb_critics/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dwb_critics 4 | 0.3.1 5 | 6 | Implementations for dwb_local_planner TrajectoryCritic interface 7 | 8 | 9 | David V. Lu!! 10 | David V. Lu!! 11 | 12 | BSD 13 | catkin 14 | angles 15 | costmap_queue 16 | dwb_local_planner 17 | geometry_msgs 18 | nav_2d_msgs 19 | nav_2d_utils 20 | nav_core2 21 | nav_grid_iterators 22 | pluginlib 23 | roscpp 24 | sensor_msgs 25 | roslint 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /dwb_critics/src/alignment_util.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Locus Robotics 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 copyright holder 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 HOLDER 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 dwb_critics 38 | { 39 | geometry_msgs::Pose2D getForwardPose(const geometry_msgs::Pose2D& pose, double distance) 40 | { 41 | geometry_msgs::Pose2D forward_pose; 42 | forward_pose.x = pose.x + distance * cos(pose.theta); 43 | forward_pose.y = pose.y + distance * sin(pose.theta); 44 | forward_pose.theta = pose.theta; 45 | return forward_pose; 46 | } 47 | } // namespace dwb_critics 48 | -------------------------------------------------------------------------------- /dwb_critics/src/twirling.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Locus Robotics 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 copyright holder 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 HOLDER 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 | #include 37 | 38 | namespace dwb_critics 39 | { 40 | void TwirlingCritic::onInit() 41 | { 42 | // Scale is set to 0 by default, so if it was not set otherwise, set to 0 43 | if (!critic_nh_.hasParam("scale")) 44 | { 45 | scale_ = 0.0; 46 | } 47 | } 48 | 49 | double TwirlingCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj) 50 | { 51 | return fabs(traj.velocity.theta); // add cost for making the robot spin 52 | } 53 | } // namespace dwb_critics 54 | 55 | PLUGINLIB_EXPORT_CLASS(dwb_critics::TwirlingCritic, dwb_local_planner::TrajectoryCritic) 56 | -------------------------------------------------------------------------------- /dwb_local_planner/include/dwb_local_planner/backwards_compatibility.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Locus Robotics 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 copyright holder 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 HOLDER 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 DWB_LOCAL_PLANNER_BACKWARDS_COMPATIBILITY_H 35 | #define DWB_LOCAL_PLANNER_BACKWARDS_COMPATIBILITY_H 36 | 37 | #include 38 | #include 39 | 40 | namespace dwb_local_planner 41 | { 42 | 43 | std::string getBackwardsCompatibleDefaultGenerator(const ros::NodeHandle& nh); 44 | 45 | /** 46 | * @brief Load parameters to emulate dwa_local_planner 47 | * 48 | * If no critic parameters are specified, this function should be called 49 | * to load/move parameters that will emulate the behavior of dwa_local_planner 50 | * 51 | * @param nh NodeHandle to load parameters from 52 | */ 53 | void loadBackwardsCompatibleParameters(const ros::NodeHandle& nh); 54 | } // namespace dwb_local_planner 55 | #endif // DWB_LOCAL_PLANNER_BACKWARDS_COMPATIBILITY_H 56 | -------------------------------------------------------------------------------- /dwb_local_planner/launch/plan_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | wait_for_transform: false 5 | pose_update_frequency: -1.0 6 | update_frequency: -1.0 7 | origin_x: -5.0 8 | origin_y: -5.0 9 | global_frame: /odom 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /dwb_local_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dwb_local_planner 4 | 0.3.1 5 | 6 | Plugin based local planner implementing the nav_core2::LocalPlanner interface. 7 | 8 | 9 | David V. Lu!! 10 | David V. Lu!! 11 | 12 | BSD 13 | 14 | catkin 15 | dwb_msgs 16 | geometry_msgs 17 | nav_2d_msgs 18 | nav_2d_utils 19 | nav_core2 20 | nav_msgs 21 | pluginlib 22 | roscpp 23 | sensor_msgs 24 | tf 25 | visualization_msgs 26 | roslint 27 | rostest 28 | rosunit 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /dwb_local_planner/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /dwb_local_planner/src/planner_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Locus Robotics 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 copyright holder 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 HOLDER 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 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | int main(int argc, char** argv) 42 | { 43 | ros::init(argc, argv, "plan_node"); 44 | ros::NodeHandle private_nh("~"); 45 | 46 | dwb_local_planner::DebugDWBLocalPlanner planner; 47 | ROS_INFO("Plan Node"); 48 | 49 | TFListenerPtr tf = std::make_shared(); 50 | tf2_ros::TransformListener tf2(*tf); 51 | 52 | pluginlib::ClassLoader costmap_loader("nav_core2", "nav_core2::Costmap"); 53 | std::string costmap_class; 54 | private_nh.param("local_costmap_class", costmap_class, std::string("nav_core_adapter::CostmapAdapter")); 55 | nav_core2::Costmap::Ptr costmap = costmap_loader.createUniqueInstance(costmap_class); 56 | costmap->initialize(private_nh, "costmap", tf); 57 | 58 | planner.initialize(private_nh, "dwb_local_planner", tf, costmap); 59 | ros::spin(); 60 | } 61 | -------------------------------------------------------------------------------- /dwb_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(dwb_msgs) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS geometry_msgs nav_2d_msgs message_generation nav_msgs 6 | ) 7 | 8 | add_message_files(FILES 9 | CriticScore.msg 10 | LocalPlanEvaluation.msg 11 | Trajectory2D.msg 12 | TrajectoryScore.msg 13 | ) 14 | add_service_files(FILES 15 | DebugLocalPlan.srv 16 | GenerateTrajectory.srv 17 | GenerateTwists.srv 18 | GetCriticScore.srv 19 | ScoreTrajectory.srv 20 | ) 21 | generate_messages(DEPENDENCIES geometry_msgs nav_2d_msgs nav_msgs) 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS geometry_msgs message_runtime nav_2d_msgs nav_msgs 25 | ) 26 | -------------------------------------------------------------------------------- /dwb_msgs/msg/CriticScore.msg: -------------------------------------------------------------------------------- 1 | # The result from one critic scoring a Twist. 2 | # Name of the critic 3 | string name 4 | # Score for the critic, not multiplied by the scale 5 | float32 raw_score 6 | # Scale for the critic, multiplied by the raw_score and added to the total score 7 | float32 scale 8 | -------------------------------------------------------------------------------- /dwb_msgs/msg/LocalPlanEvaluation.msg: -------------------------------------------------------------------------------- 1 | # Full Scoring for running the local planner 2 | 3 | # Header, used for timestamp 4 | Header header 5 | # All the trajectories evaluated and their scores 6 | TrajectoryScore[] twists 7 | # Convenience index of the best (lowest) score in the twists array 8 | uint16 best_index 9 | # Convenience index of the worst (highest) score in the twists array. Useful for scaling. 10 | uint16 worst_index 11 | -------------------------------------------------------------------------------- /dwb_msgs/msg/Trajectory2D.msg: -------------------------------------------------------------------------------- 1 | # For a given velocity command, the poses that the robot will go to in the allotted time. 2 | 3 | # Input Velocity 4 | nav_2d_msgs/Twist2D velocity 5 | 6 | # Poses the robot will go to, given our kinematic model 7 | geometry_msgs/Pose2D[] poses 8 | 9 | # Time difference between the current pose and the poses in the trajectory. 10 | # Parallel array to poses. Length should be the same. 11 | duration[] time_offsets 12 | -------------------------------------------------------------------------------- /dwb_msgs/msg/TrajectoryScore.msg: -------------------------------------------------------------------------------- 1 | # Complete scoring for a given twist. 2 | 3 | # The trajectory being scored 4 | Trajectory2D traj 5 | # The Scores for each of the critics employed 6 | CriticScore[] scores 7 | # Convenience member that totals the critic scores 8 | float32 total 9 | -------------------------------------------------------------------------------- /dwb_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dwb_msgs 4 | 0.3.1 5 | Message/Service definitions specifically for the dwb_local_planner 6 | 7 | David V. Lu!! 8 | 9 | BSD 10 | 11 | catkin 12 | geometry_msgs 13 | nav_2d_msgs 14 | nav_msgs 15 | message_generation 16 | message_runtime 17 | message_runtime 18 | 19 | 20 | -------------------------------------------------------------------------------- /dwb_msgs/srv/DebugLocalPlan.srv: -------------------------------------------------------------------------------- 1 | # For a given pose velocity and global_plan, run the local planner and return full results 2 | nav_2d_msgs/Pose2DStamped pose 3 | nav_2d_msgs/Twist2D velocity 4 | nav_2d_msgs/Path2D global_plan 5 | nav_2d_msgs/Pose2DStamped goal 6 | --- 7 | LocalPlanEvaluation results 8 | -------------------------------------------------------------------------------- /dwb_msgs/srv/GenerateTrajectory.srv: -------------------------------------------------------------------------------- 1 | # For a given start pose, velocity and desired velocity, generate which poses will be visited 2 | geometry_msgs/Pose2D start_pose 3 | nav_2d_msgs/Twist2D start_vel 4 | nav_2d_msgs/Twist2D cmd_vel 5 | --- 6 | Trajectory2D traj 7 | -------------------------------------------------------------------------------- /dwb_msgs/srv/GenerateTwists.srv: -------------------------------------------------------------------------------- 1 | # For a given velocity, generate which twist commands will be evaluated 2 | nav_2d_msgs/Twist2D current_vel 3 | --- 4 | nav_2d_msgs/Twist2D[] twists 5 | -------------------------------------------------------------------------------- /dwb_msgs/srv/GetCriticScore.srv: -------------------------------------------------------------------------------- 1 | nav_2d_msgs/Pose2DStamped pose 2 | nav_2d_msgs/Twist2D velocity 3 | nav_2d_msgs/Path2D global_plan 4 | nav_2d_msgs/Pose2DStamped goal 5 | Trajectory2D traj 6 | string critic_name 7 | --- 8 | CriticScore score 9 | -------------------------------------------------------------------------------- /dwb_msgs/srv/ScoreTrajectory.srv: -------------------------------------------------------------------------------- 1 | nav_2d_msgs/Pose2DStamped pose 2 | nav_2d_msgs/Twist2D velocity 3 | nav_2d_msgs/Path2D global_plan 4 | nav_2d_msgs/Pose2DStamped goal 5 | Trajectory2D traj 6 | --- 7 | TrajectoryScore score 8 | -------------------------------------------------------------------------------- /dwb_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(dwb_plugins) 3 | 4 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror") 5 | if(NOT "${CMAKE_CXX_STANDARD}") 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | find_package(catkin REQUIRED COMPONENTS angles dwb_local_planner dynamic_reconfigure nav_2d_msgs nav_2d_utils nav_core2 pluginlib roscpp) 10 | 11 | generate_dynamic_reconfigure_options(cfg/KinematicParams.cfg) 12 | 13 | catkin_package( 14 | INCLUDE_DIRS include 15 | CATKIN_DEPENDS angles dwb_local_planner dynamic_reconfigure nav_2d_msgs nav_2d_utils nav_core2 pluginlib roscpp 16 | LIBRARIES simple_goal_checker stopped_goal_checker standard_traj_generator 17 | ) 18 | 19 | include_directories( 20 | include ${catkin_INCLUDE_DIRS} 21 | ) 22 | 23 | add_library(simple_goal_checker src/simple_goal_checker.cpp) 24 | target_link_libraries(simple_goal_checker ${catkin_LIBRARIES}) 25 | add_dependencies(simple_goal_checker ${catkin_EXPORTED_TARGETS} ${dwb_plugins_EXPORTED_TARGETS}) 26 | 27 | add_library(stopped_goal_checker src/stopped_goal_checker.cpp) 28 | target_link_libraries(stopped_goal_checker simple_goal_checker ${catkin_LIBRARIES}) 29 | add_dependencies(stopped_goal_checker ${catkin_EXPORTED_TARGETS} ${dwb_plugins_EXPORTED_TARGETS}) 30 | 31 | add_library(standard_traj_generator 32 | src/standard_traj_generator.cpp 33 | src/limited_accel_generator.cpp 34 | src/kinematic_parameters.cpp 35 | src/xy_theta_iterator.cpp) 36 | target_link_libraries(standard_traj_generator ${catkin_LIBRARIES}) 37 | add_dependencies(standard_traj_generator ${catkin_EXPORTED_TARGETS} ${dwb_plugins_EXPORTED_TARGETS}) 38 | 39 | if (CATKIN_ENABLE_TESTING) 40 | catkin_add_gtest(vtest test/velocity_iterator_test.cpp) 41 | 42 | find_package(rostest REQUIRED) 43 | add_rostest_gtest(goal_checker test/goal_checker.launch test/goal_checker.cpp) 44 | target_link_libraries(goal_checker simple_goal_checker stopped_goal_checker ${GTEST_LIBRARIES}) 45 | 46 | add_rostest_gtest(twist_gen_test test/twist_gen.launch test/twist_gen.cpp) 47 | target_link_libraries(twist_gen_test standard_traj_generator ${GTEST_LIBRARIES}) 48 | 49 | find_package(roslint REQUIRED) 50 | roslint_cpp() 51 | roslint_add_test() 52 | endif (CATKIN_ENABLE_TESTING) 53 | 54 | install(TARGETS simple_goal_checker stopped_goal_checker standard_traj_generator 55 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 56 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 57 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 58 | ) 59 | install(DIRECTORY include/${PROJECT_NAME}/ 60 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 61 | ) 62 | install(FILES plugins.xml 63 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 64 | ) 65 | -------------------------------------------------------------------------------- /dwb_plugins/cfg/KinematicParams.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t 3 | 4 | gen = ParameterGenerator() 5 | 6 | # velocities 7 | gen.add('min_vel_x', double_t, 0, 'The minimum x velocity for the robot in m/s', 0.0) 8 | gen.add('max_vel_x', double_t, 0, 'The maximum x velocity for the robot in m/s', 0.55) 9 | gen.add('min_vel_y', double_t, 0, 'The minimum y velocity for the robot in m/s', -0.1) 10 | gen.add('max_vel_y', double_t, 0, 'The maximum y velocity for the robot in m/s', 0.1) 11 | gen.add('max_vel_theta', double_t, 0, 'The absolute value of the maximum rotational velocity for the robot in rad/s. ' 12 | 'The minimum rotational velocity is assumed to be -max_vel_theta', 1.0) 13 | 14 | # acceleration 15 | gen.add('acc_lim_x', double_t, 0, 'The acceleration limit of the robot in the x direction in m/s^2', 2.5) 16 | gen.add('acc_lim_y', double_t, 0, 'The acceleration limit of the robot in the y direction in m/s^2', 2.5) 17 | gen.add('acc_lim_theta', double_t, 0, 'The acceleration limit of the robot in the theta direction in rad/s^2', 3.2) 18 | 19 | gen.add('decel_lim_x', double_t, 0, 'The deceleration limit of the robot in the x direction in m/s^2', -2.5) 20 | gen.add('decel_lim_y', double_t, 0, 'The deceleration limit of the robot in the y direction in m/s^2', -2.5) 21 | gen.add('decel_lim_theta', double_t, 0, 'The deceleration limit of the robot in the theta direction in rad/s^2', -3.2) 22 | 23 | gen.add('min_speed_xy', double_t, 0, 'The absolute value of the minimum translational/xy velocity in m/s. ' 24 | 'If the value is negative, then the min speed will be arbitrarily close to 0.0. ' 25 | 'Previously called min_trans_vel', 0.1) 26 | gen.add('max_speed_xy', double_t, 0, 'The absolute value of the maximum translational/xy velocity in m/s. ' 27 | 'If the value is negative, then the max speed is hypot(max_vel_x, max_vel_y). ' 28 | 'Previously called max_trans_vel', 0.55) 29 | gen.add('min_speed_theta', double_t, 0, 'The absolute value of the minimum rotational velocity in rad/s. ' 30 | 'If the value is negative, then the min speed will be arbitrarily close to 0.0.' 31 | ' Previously called min_rot_vel', 0.4) 32 | 33 | exit(gen.generate('dwb_plugins', 'dwb_plugins', 'KinematicParams')) 34 | -------------------------------------------------------------------------------- /dwb_plugins/doc/VelocitySpace.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/dwb_plugins/doc/VelocitySpace.png -------------------------------------------------------------------------------- /dwb_plugins/doc/lim_pv.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/dwb_plugins/doc/lim_pv.png -------------------------------------------------------------------------------- /dwb_plugins/doc/std_pv.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/dwb_plugins/doc/std_pv.png -------------------------------------------------------------------------------- /dwb_plugins/include/dwb_plugins/stopped_goal_checker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Locus Robotics 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 copyright holder 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 HOLDER 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 | #ifndef DWB_PLUGINS_STOPPED_GOAL_CHECKER_H 36 | #define DWB_PLUGINS_STOPPED_GOAL_CHECKER_H 37 | 38 | #include 39 | #include 40 | 41 | namespace dwb_plugins 42 | { 43 | 44 | /** 45 | * @class StoppedGoalChecker 46 | * @brief Goal Checker plugin that checks the position difference and velocity 47 | */ 48 | class StoppedGoalChecker : public SimpleGoalChecker 49 | { 50 | public: 51 | StoppedGoalChecker(); 52 | // Standard GoalChecker Interface 53 | void initialize(const ros::NodeHandle& nh) override; 54 | bool isGoalReached(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose, 55 | const nav_2d_msgs::Twist2D& velocity) override; 56 | protected: 57 | double rot_stopped_velocity_, trans_stopped_velocity_; 58 | }; 59 | 60 | } // namespace dwb_plugins 61 | 62 | #endif // DWB_PLUGINS_STOPPED_GOAL_CHECKER_H 63 | -------------------------------------------------------------------------------- /dwb_plugins/include/dwb_plugins/velocity_iterator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Locus Robotics 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 copyright holder 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 HOLDER 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 | #ifndef DWB_PLUGINS_VELOCITY_ITERATOR_H 36 | #define DWB_PLUGINS_VELOCITY_ITERATOR_H 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | namespace dwb_plugins 43 | { 44 | class VelocityIterator 45 | { 46 | public: 47 | virtual ~VelocityIterator() {} 48 | virtual void initialize(ros::NodeHandle& nh, KinematicParameters::Ptr kinematics) = 0; 49 | virtual void startNewIteration(const nav_2d_msgs::Twist2D& current_velocity, double dt) = 0; 50 | virtual bool hasMoreTwists() = 0; 51 | virtual nav_2d_msgs::Twist2D nextTwist() = 0; 52 | }; 53 | } // namespace dwb_plugins 54 | 55 | #endif // DWB_PLUGINS_VELOCITY_ITERATOR_H 56 | -------------------------------------------------------------------------------- /dwb_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dwb_plugins 4 | 0.3.1 5 | 6 | Standard implementations of the GoalChecker 7 | and TrajectoryGenerators for dwb_local_planner 8 | 9 | 10 | David V. Lu!! 11 | 12 | BSD 13 | 14 | catkin 15 | angles 16 | dwb_local_planner 17 | dynamic_reconfigure 18 | nav_2d_msgs 19 | nav_2d_utils 20 | nav_core2 21 | pluginlib 22 | roscpp 23 | roslint 24 | rostest 25 | rosunit 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /dwb_plugins/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /dwb_plugins/test/goal_checker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /dwb_plugins/test/twist_gen.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /global_planner_tests/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package global_planner_tests 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.4 (2018-06-06) 6 | ------------------ 7 | 8 | 0.1.3 (2018-04-25) 9 | ------------------ 10 | 11 | 0.1.2 (2018-04-24) 12 | ------------------ 13 | 14 | 0.1.1 (2018-04-19) 15 | ------------------ 16 | 17 | 0.1.0 (2018-04-13) 18 | ------------------ 19 | 20 | 0.0.7 (2018-04-09) 21 | ------------------ 22 | 23 | 0.0.6 (2018-02-15) 24 | ------------------ 25 | 26 | 0.0.5 (2018-02-01) 27 | ------------------ 28 | * Bump version number 29 | * Global Planner Exceptions and Tests (`#19 `_) 30 | * Global Planner Exceptions and Tests 31 | * Code review fixes v.1 32 | * Additional exception constructors 33 | * Add roslint tests 34 | * Code Review 35 | * Update dependencies/installs 36 | * Const refs for everyone 37 | * Contributors: David V. Lu, David V. Lu!! 38 | 39 | 0.0.4 (2018-01-26) 40 | ------------------ 41 | 42 | 0.0.3 (2017-11-08) 43 | ------------------ 44 | 45 | 0.0.2 (2017-10-12) 46 | ------------------ 47 | 48 | 0.0.1 (2017-10-10) 49 | ------------------ 50 | -------------------------------------------------------------------------------- /global_planner_tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(global_planner_tests) 3 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror") 4 | if(NOT "${CMAKE_CXX_STANDARD}") 5 | set(CMAKE_CXX_STANDARD 14) 6 | endif() 7 | 8 | find_package(catkin REQUIRED COMPONENTS map_server nav_core2 roscpp nav_msgs pluginlib) 9 | 10 | catkin_package( 11 | CATKIN_DEPENDS map_server nav_core2 roscpp nav_msgs pluginlib 12 | INCLUDE_DIRS include 13 | LIBRARIES ${PROJECT_NAME} 14 | ) 15 | 16 | add_library(${PROJECT_NAME} src/global_planner_tests.cpp src/easy_costmap.cpp src/many_map_test_suite.cpp src/util.cpp) 17 | target_link_libraries( 18 | ${PROJECT_NAME} ${catkin_LIBRARIES} yaml-cpp 19 | ) 20 | 21 | add_executable(gpt_node src/gpt_node.cpp) 22 | target_link_libraries(gpt_node ${PROJECT_NAME} ${catkin_LIBRARIES}) 23 | 24 | add_executable(heatmap_node src/heatmap_node.cpp) 25 | target_link_libraries(heatmap_node ${PROJECT_NAME} ${catkin_LIBRARIES}) 26 | 27 | add_executable(many_map_node src/many_map_node.cpp) 28 | target_link_libraries(many_map_node ${PROJECT_NAME} ${catkin_LIBRARIES}) 29 | 30 | include_directories( 31 | include ${catkin_INCLUDE_DIRS} 32 | ) 33 | 34 | if(CATKIN_ENABLE_TESTING) 35 | find_package(roslint REQUIRED) 36 | roslint_cpp() 37 | roslint_add_test() 38 | endif() 39 | 40 | install( 41 | DIRECTORY include/${PROJECT_NAME}/ 42 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 43 | ) 44 | install( 45 | TARGETS global_planner_tests 46 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 47 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 48 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 49 | ) 50 | install(TARGETS gpt_node heatmap_node many_map_node 51 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 52 | ) 53 | install(DIRECTORY maps/ 54 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/maps 55 | ) 56 | install(FILES config/standard_tests.yaml 57 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config 58 | ) 59 | -------------------------------------------------------------------------------- /global_planner_tests/config/standard_tests.yaml: -------------------------------------------------------------------------------- 1 | standard_prefix: package://global_planner_tests/maps/ 2 | full_coverage_maps: 3 | - empty.png 4 | - smile.png 5 | - smallloop.png 6 | - unknown.png 7 | - corner.png 8 | - inflated.png 9 | no_coverage_maps: 10 | - nopaths.png 11 | -------------------------------------------------------------------------------- /global_planner_tests/include/global_planner_tests/util.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Locus Robotics 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 copyright holder 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 HOLDER 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 | #ifndef GLOBAL_PLANNER_TESTS_UTIL_H 36 | #define GLOBAL_PLANNER_TESTS_UTIL_H 37 | #include 38 | 39 | namespace global_planner_tests 40 | { 41 | /** 42 | * @brief Replace a package:// prefix with the actual path to the given package 43 | * 44 | * This is a quick dirty way to resolve package names to strings. 45 | * The alternative is resource_retriever, but that returns a file object as opposed to the updated filename. 46 | * 47 | * @param filename input file path 48 | * @return updated file path 49 | */ 50 | std::string resolve_filename(const std::string& filename); 51 | 52 | } // namespace global_planner_tests 53 | 54 | #endif // GLOBAL_PLANNER_TESTS_UTIL_H 55 | -------------------------------------------------------------------------------- /global_planner_tests/maps/corner.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/global_planner_tests/maps/corner.png -------------------------------------------------------------------------------- /global_planner_tests/maps/empty.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/global_planner_tests/maps/empty.png -------------------------------------------------------------------------------- /global_planner_tests/maps/inflated.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/global_planner_tests/maps/inflated.png -------------------------------------------------------------------------------- /global_planner_tests/maps/nopaths.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/global_planner_tests/maps/nopaths.png -------------------------------------------------------------------------------- /global_planner_tests/maps/smallloop.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/global_planner_tests/maps/smallloop.png -------------------------------------------------------------------------------- /global_planner_tests/maps/smile.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/global_planner_tests/maps/smile.png -------------------------------------------------------------------------------- /global_planner_tests/maps/unknown.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/global_planner_tests/maps/unknown.png -------------------------------------------------------------------------------- /global_planner_tests/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | global_planner_tests 4 | 0.3.1 5 | 6 | A collection of tests for checking the validity and completeness of global planners. 7 | 8 | 9 | David V. Lu!! 10 | 11 | BSD 12 | 13 | catkin 14 | map_server 15 | nav_core2 16 | nav_msgs 17 | pluginlib 18 | roscpp 19 | yaml-cpp 20 | roslint 21 | 22 | 23 | -------------------------------------------------------------------------------- /locomotor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(locomotor) 3 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror") 4 | if(NOT "${CMAKE_CXX_STANDARD}") 5 | set(CMAKE_CXX_STANDARD 14) 6 | endif() 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | actionlib geometry_msgs locomotor_msgs nav_2d_msgs nav_2d_utils nav_core2 nav_msgs pluginlib roscpp rospy 10 | ) 11 | 12 | catkin_package( 13 | INCLUDE_DIRS include 14 | CATKIN_DEPENDS 15 | actionlib 16 | geometry_msgs 17 | locomotor_msgs 18 | nav_2d_msgs 19 | nav_2d_utils 20 | nav_core2 21 | nav_msgs 22 | pluginlib 23 | roscpp 24 | rospy 25 | LIBRARIES locomotor 26 | ) 27 | 28 | add_library( 29 | locomotor 30 | src/locomotor.cpp 31 | src/executor.cpp 32 | src/publishers.cpp 33 | src/locomotor_action_server.cpp 34 | ) 35 | target_link_libraries( 36 | locomotor ${catkin_LIBRARIES} 37 | ) 38 | 39 | add_executable(locomotor1 src/single_thread_locomotor.cpp) 40 | target_link_libraries( 41 | locomotor1 locomotor ${catkin_LIBRARIES} 42 | ) 43 | 44 | add_executable(locomotor2 src/double_thread_locomotor.cpp) 45 | target_link_libraries( 46 | locomotor2 locomotor ${catkin_LIBRARIES} 47 | ) 48 | 49 | include_directories( 50 | include ${catkin_INCLUDE_DIRS} 51 | ) 52 | 53 | if(CATKIN_ENABLE_TESTING) 54 | find_package(roslint REQUIRED) 55 | roslint_cpp() 56 | roslint_add_test() 57 | endif() 58 | 59 | install(TARGETS locomotor1 locomotor2 60 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 61 | ) 62 | install(TARGETS locomotor 63 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 64 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 65 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 66 | ) 67 | install(DIRECTORY include/${PROJECT_NAME}/ 68 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 69 | ) 70 | catkin_install_python(PROGRAMS scripts/send_action.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 71 | -------------------------------------------------------------------------------- /locomotor/README.md: -------------------------------------------------------------------------------- 1 | # Locomotor 2 | 3 | Locomotor is an extensible path planning coordination engine that replaces `move_base`. The goal is to provide a mechanism for controlling what happens when the global and local planners succeed and fail. It leverages ROS callback queues to coordinate multiple threads. 4 | 5 | * [Motivation](doc/Motivation.md) 6 | * [Primary Design](doc/PrimaryDesign.md) 7 | * [Example State Machines](doc/ExampleStateMachines.md) 8 | * [Error Handling](doc/ErrorHandling.md) 9 | * [Build on Top](doc/BuildingOnTop.md) 10 | -------------------------------------------------------------------------------- /locomotor/doc/BuildingOnTop.md: -------------------------------------------------------------------------------- 1 | ## Building on the Events 2 | ### Action Structure 3 | Although Locomotor is a replacement for move_base, it does not use the same action definition. The existing `move_base_msgs/MoveBase.action` is wholly inadequate for almost anything useful. 4 | 5 | ``` 6 | # move_base_msgs/MoveBase.action 7 | geometry_msgs/PoseStamped target_pose 8 | --- 9 | --- 10 | geometry_msgs/PoseStamped base_position 11 | ``` 12 | 13 | You provide a goal pose, and the only feedback you get is the robot's current position. You do get a notification when the action is done, so that's nice. 14 | 15 | [`locomotor_msgs`](../locomotor_msgs) provides a much richer action definition, although you can use your own action defintions in conjunction with Locomotor. 16 | 17 | 18 | ### Action Server 19 | This package also provides the `LocomotorActionServer` class the enables `actionlib` to be integrated with the state machines. It generates the appropriate feedback and results, to be published when particular events are called. 20 | 21 | ![actionlib flow diagram](figures/action_server.png) 22 | 23 | The above diagram is the same as the single thread version but also shows interaction with the ActionServer in the leftmost column. 24 | 25 | ### ROS API vs C++ API 26 | The core Locomotor code is strictly C++ based for maximum flexibility. Now there's a fundamental question here as to why we even expose a C++ API at all. Why not just have everything be action based? In this particular case, ROS's message system may not adequately cover the richness of the extensible classes that C++ provides. 27 | 28 | In particular, one could not possibly define all the plausible error codes within a message/action definition. Such a list would not include specific errors that only occur in certain problem domains that other people's extensions of locomotor might require. 29 | 30 | By implementing a custom state machine around Locomotor, one can define their own schema for error codes and the like, and publish feedback in a way compatible with the specific problem domain. That is why a specific action interface is not baked into the core `Locomotor` class, so that developers can define/choose their own action definition. 31 | -------------------------------------------------------------------------------- /locomotor/doc/ErrorHandling.md: -------------------------------------------------------------------------------- 1 | # Error Handling 2 | Let's take a precise look at what the error handling functions look like. 3 | 4 | ``` 5 | using PlannerExceptionCallback = std::function; 6 | ``` 7 | 8 | There are several standard extensions for the base `PlannerException` class in `nav_core2/exceptions.h` that will be passed as an exception pointer. When the function is called, you should be able to check the class of the exception against your known types and react accordingly. For instance, 9 | 10 | ``` 11 | void SomeStateMachine::onGlobalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, 12 | const ros::Duration& planning_time) 13 | { 14 | try 15 | { 16 | std::rethrow_exception(e_ptr); 17 | } 18 | catch (nav_core2::OccupiedGoalException& e) 19 | { 20 | // modify the goal and try again 21 | } 22 | catch (nav_core2::PlannerTFException& e) 23 | { 24 | // wait 10 seconds, try again 25 | } 26 | catch (nav_core2::PlannerException& e) // catch all 27 | { 28 | // trigger plan failure 29 | } 30 | } 31 | ``` 32 | 33 | Importantly, the individual planners can implement their own extensions of `PlannerException` and have their version of `StateMachine` handle it however they like. 34 | 35 | ``` 36 | void LocomotorBrown::onGlobalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, 37 | const ros::Duration& planning_time) 38 | { 39 | try 40 | { 41 | std::rethrow_exception(e_ptr); 42 | } 43 | catch (LocomotorBrown::TimeTravelException& e) 44 | { 45 | // recalibrate flux capacitor 46 | } 47 | ... 48 | } 49 | ``` 50 | 51 | 52 | ## Recovery Behaviors 53 | There are essentially two types of Recovery Behaviors. 54 | * Simple near-instantaneous actions, like clearing the costmap 55 | * Prolonged actions that attempt to clear things up (e.g. rotating in place for a bit) 56 | 57 | The former can be handled in the error event handling. The latter could be represented as switching to a new planner for some set amount of navigation. 58 | 59 | ![error handling flow diagram](figures/error_handling.png) 60 | 61 | Alternatively, the `locomove_base` package is an implementation of Locomotor that uses the standard `nav_core::RecoveryBehavior` configurations that `move_base` uses for error handling. 62 | -------------------------------------------------------------------------------- /locomotor/doc/ExampleStateMachines.md: -------------------------------------------------------------------------------- 1 | # Example State machines 2 | 3 | # One Thread To Rule Them All 4 | The simplest version of Locomotor is single threaded and works like this (assuming all goes to plan, pun intended). 5 | 1. When a new goal is received, update the global costmap 6 | 2. When that is completed, create a new global plan (`makePlan`) 7 | 3. When that is completed, pass the global plan to the local planner (`setPlan`) 8 | 4. Then update the local costmap. 9 | 5. When that is completed, create a new local plan (`computeVelocityCommands`) 10 | 6. Repeat steps 4 and 5 until the destination is reached 11 | 12 | ![single thread flow diagram](figures/single_thread.png) 13 | 14 | One main thread would handle all of events and request additional events. 15 | 16 | In this super-simple version, we'll just handle errors very conservatively. 17 | * If the global costmap fails to update, or the global planning fails, we fail the whole navigation 18 | * If the local costmap fails to update, then we send a zero command velocity and try again 19 | * If the local planning fails, we send a zero command velocity AND trigger another global planning 20 | 21 | This example is implemented in SingleThreadLocomotor using a timer to repeatedly do steps 4 and 5. 22 | 23 | 24 | # It Takes Two (Threads) 25 | The simple version would be unable to continuously create global plans without blocking the local planner. So you could also make a two `Executor` version that had similar logic. 26 | ![two thread flow diagram](figures/two_thread.png) 27 | 28 | The main difference is that there are two callback queues, one for the local costmap and planner and one for the global costmap and planner. The main point of crossover is that `onNewGlobalPlan` is put onto the local callback queue so that it can inform the local planner of the new plan. 29 | 30 | This example is implemented in DoubleThreadLocomotor with two timers: one for triggering global costmap updates, and one that triggers local costmap updates. 31 | 32 | # Other Configurations 33 | You could also set up a four `Executor` version that triggers costmap updates and planning on fixed time cycles. However, that is not shown. 34 | -------------------------------------------------------------------------------- /locomotor/doc/MultiplePlanners.md: -------------------------------------------------------------------------------- 1 | # Multiple Planners 2 | Locomotor can load any number of (local and global) planners into different namespaces. However, only one is marked as active at any particular time. This allows for easy switching between planners, done using the string namespace. 3 | 4 | One could easily imagine handling different types of Goals by first setting which planners to use, i.e. if you receive a Docking goal, you could set the local planner to the docking local planner and then attempt to dock. 5 | -------------------------------------------------------------------------------- /locomotor/doc/figures/action_server.flow: -------------------------------------------------------------------------------- 1 | title Action Server 2 | participant ActionServer as as 3 | participant Main Thread as ml 4 | participant Local Planner as lp 5 | participant Local Costmap as lc 6 | participant Global Planner as gp 7 | participant Global Costmap as gc 8 | as -> ml: onGoal 9 | ml -> +gc: requestGlobalCostmapUpdate 10 | gc -> -ml: onGlobalCostmapUpdate 11 | ml -> +gp: requestGlobalPlan 12 | gp -> -ml: onNewGlobalPlan 13 | ml -> as: sendFeedback 14 | ml -> lp: setPlan 15 | ml -> +lc: requestLocalCostmapUpdate 16 | lc -> -ml: onLocalCostmapUpdate 17 | ml -> +lp: requestLocalPlan 18 | lp -> -ml: onNewLocalPlan 19 | ml -> as: sendFeedback 20 | ml -> +lc: doCostmapUpdate 21 | lc -> -ml: onLocalCostmapUpdate 22 | ml -> +lp: requestLocalPlan 23 | lp -> -ml: onNewLocalPlan 24 | ml -> as: sendFeedback 25 | ml -> ml: ... 26 | lp -> ml: onNavigationCompleted 27 | ml -> as: sendResult 28 | -------------------------------------------------------------------------------- /locomotor/doc/figures/action_server.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/locomotor/doc/figures/action_server.png -------------------------------------------------------------------------------- /locomotor/doc/figures/error_handling.flow: -------------------------------------------------------------------------------- 1 | title Error Handling 2 | participant Main Thread as ml 3 | participant Local Planner as lp 4 | participant Local Costmap as lc 5 | ml -> ml: ... 6 | ml -> +lp: requestLocalPlan 7 | lp -> -ml: onLocalPlanningException 8 | ml -> lc: clearCostmap 9 | ml -> +lc: requestLocalCostmapUpdate 10 | lc -> -ml: onLocalCostmapUpdate 11 | ml -> +lp: requestLocalPlan 12 | lp -> -ml: onNewLocalPlan 13 | -------------------------------------------------------------------------------- /locomotor/doc/figures/error_handling.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/locomotor/doc/figures/error_handling.png -------------------------------------------------------------------------------- /locomotor/doc/figures/generate.py: -------------------------------------------------------------------------------- 1 | import os 2 | import requests 3 | import re 4 | import urllib 5 | 6 | WSD_URL = 'http://www.websequencediagrams.com/' 7 | 8 | def getSequenceDiagram(text, out_fn, style='modern-blue'): 9 | request = {} 10 | request["message"] = text 11 | request["style"] = style 12 | request["apiVersion"] = "1" 13 | 14 | r = requests.post(WSD_URL, data=request) 15 | response = r.json() 16 | if 'img' not in response: 17 | print("Invalid response from server.") 18 | return 19 | 20 | with open(out_fn, 'w') as f: 21 | print out_fn 22 | r = requests.get(WSD_URL + response['img']) 23 | f.write(r.content) 24 | 25 | 26 | if __name__ == '__main__': 27 | current_directory = os.path.abspath(os.path.dirname(__file__)) 28 | for fn in os.listdir(current_directory): 29 | base, ext = os.path.splitext(fn) 30 | if ext != '.flow': 31 | continue 32 | text = open(os.path.join(current_directory, fn)).read() 33 | out_fn = os.path.join(current_directory, base + '.png') 34 | getSequenceDiagram(text, out_fn) 35 | -------------------------------------------------------------------------------- /locomotor/doc/figures/single_thread.flow: -------------------------------------------------------------------------------- 1 | title Single threaded version 2 | participant Main Thread as ml 3 | participant Local Planner as lp 4 | participant Local Costmap as lc 5 | participant Global Planner as gp 6 | participant Global Costmap as gc 7 | ml -> +gc: requestGlobalCostmapUpdate 8 | gc -> -ml: onGlobalCostmapUpdate 9 | ml -> +gp: requestGlobalPlan 10 | gp -> -ml: onNewGlobalPlan 11 | ml -> lp: setPlan 12 | ml -> +lc: requestLocalCostmapUpdate 13 | lc -> -ml: onLocalCostmapUpdate 14 | ml -> +lp: requestLocalPlan 15 | lp -> -ml: onNewLocalPlan 16 | ml -> +lc: doCostmapUpdate 17 | lc -> -ml: onLocalCostmapUpdate 18 | ml -> +lp: requestLocalPlan 19 | lp -> -ml: onNewLocalPlan 20 | -------------------------------------------------------------------------------- /locomotor/doc/figures/single_thread.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/locomotor/doc/figures/single_thread.png -------------------------------------------------------------------------------- /locomotor/doc/figures/two_thread.flow: -------------------------------------------------------------------------------- 1 | title Two threaded version 2 | 3 | participant Local Costmap as lc 4 | participant Local Planner as lp 5 | participant Local Thread as ml 6 | participant Global Thread as gt 7 | participant Global Planner as gp 8 | participant Global Costmap as gc 9 | 10 | gt -> +gc: requestGlobalCostmapUpdate 11 | gc -> -gt: onGlobalCostmapUpdate 12 | gt -> +gp: requestGlobalPlan 13 | gp -> -ml: onNewGlobalPlan 14 | ml -> lp: setPlan 15 | ml -> +lc: requestLocalCostmapUpdate 16 | gt -> +gc: requestGlobalCostmapUpdate 17 | lc -> -ml: onLocalCostmapUpdate 18 | ml -> +lp: requestLocalPlan 19 | lp -> -ml: onNewLocalPlan 20 | ml -> +lc: requestLocalCostmapUpdate 21 | gc -> -gt: onGlobalCostmapUpdate 22 | gt -> +gp: requestGlobalPlan 23 | lc -> -ml: onLocalCostmapUpdate 24 | ml -> +lp: requestLocalPlan 25 | gp -> -ml: onNewGlobalPlan 26 | lp -> -ml: onNewLocalPlan 27 | ml -> lp: setPlan 28 | ml -> +lc: requestLocalCostmapUpdate 29 | gt -> +gc: requestGlobalCostmapUpdate 30 | lc -> -ml: onLocalCostmapUpdate 31 | ml -> +lp: requestLocalPlan 32 | lp -> -ml: onNewLocalPlan 33 | -------------------------------------------------------------------------------- /locomotor/doc/figures/two_thread.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/locomotor/doc/figures/two_thread.png -------------------------------------------------------------------------------- /locomotor/include/locomotor/publishers.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 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 copyright holder 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 HOLDER 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 | #ifndef LOCOMOTOR_PUBLISHERS_H 36 | #define LOCOMOTOR_PUBLISHERS_H 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | namespace locomotor 43 | { 44 | enum class PathType { NO_PATH, PATH_3D, PATH_2D }; 45 | enum class TwistType { NO_TWIST, TWIST_3D, TWIST_2D, TWIST_2D_STAMPED }; 46 | 47 | class PathPublisher 48 | { 49 | public: 50 | explicit PathPublisher(ros::NodeHandle& nh); 51 | void publishPath(const nav_2d_msgs::Path2D& global_plan); 52 | protected: 53 | PathType path_type_; 54 | ros::Publisher pub_; 55 | double global_plan_epsilon_; 56 | }; 57 | 58 | class TwistPublisher 59 | { 60 | public: 61 | explicit TwistPublisher(ros::NodeHandle& nh); 62 | void publishTwist(const nav_2d_msgs::Twist2DStamped& command); 63 | protected: 64 | TwistType twist_type_; 65 | ros::Publisher pub_; 66 | }; 67 | } // namespace locomotor 68 | 69 | #endif // LOCOMOTOR_PUBLISHERS_H 70 | -------------------------------------------------------------------------------- /locomotor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | locomotor 4 | 0.3.1 5 | Locomotor is an extensible path planning coordination engine that replaces move_base. The goal is to provide a mechanism for controlling what happens when the global and local planners succeed and fail. It leverages ROS callback queues to coordinate multiple threads. 6 | David V. Lu!! 7 | BSD 8 | catkin 9 | actionlib 10 | geometry_msgs 11 | locomotor_msgs 12 | nav_2d_msgs 13 | nav_2d_utils 14 | nav_core2 15 | nav_msgs 16 | pluginlib 17 | roscpp 18 | rospy 19 | roslint 20 | 21 | -------------------------------------------------------------------------------- /locomotor/scripts/send_action.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import rospy 4 | import actionlib 5 | import argparse 6 | 7 | from locomotor_msgs.msg import NavigateToPoseAction, NavigateToPoseGoal 8 | 9 | def print_feedback(feedback): 10 | pose = feedback.state.global_pose.pose 11 | vel = feedback.state.current_velocity.velocity 12 | print('%.2f %.2f %.2f | %.2f %.2f' % (pose.x, pose.y, pose.theta, 13 | vel.x, vel.theta)) 14 | print('Global plan: %d poses' % len(feedback.state.global_plan.poses)) 15 | print('%.2f %.2f %.2f' % (feedback.percent_complete, 16 | feedback.distance_traveled, 17 | feedback.estimated_distance_remaining)) 18 | 19 | 20 | parser = argparse.ArgumentParser() 21 | parser.add_argument('x', nargs='?', type=float, default=0.0) 22 | parser.add_argument('y', nargs='?', type=float, default=0.0) 23 | parser.add_argument('theta', nargs='?', type=float, default=0.0) 24 | parser.add_argument('-f', '--frame_id', default='map') 25 | parser.add_argument('-n', '--namespace', default='/locomotor/') 26 | args = parser.parse_args() 27 | 28 | rospy.init_node('send_action', anonymous=True) 29 | client = actionlib.SimpleActionClient(args.namespace + '/navigate', NavigateToPoseAction) 30 | client.wait_for_server() 31 | goal = NavigateToPoseGoal() 32 | goal.goal.pose.x = args.x 33 | goal.goal.pose.y = args.y 34 | goal.goal.pose.theta = args.theta 35 | goal.goal.header.frame_id = args.frame_id 36 | client.send_goal(goal, feedback_cb = print_feedback) 37 | client.wait_for_result() 38 | print(client.get_result()) 39 | -------------------------------------------------------------------------------- /locomotor_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(locomotor_msgs) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS actionlib_msgs message_generation nav_2d_msgs 6 | ) 7 | 8 | add_message_files(FILES 9 | NavigationState.msg 10 | ResultCode.msg 11 | ) 12 | add_action_files( 13 | FILES NavigateToPose.action 14 | ) 15 | generate_messages( 16 | DEPENDENCIES actionlib_msgs nav_2d_msgs 17 | ) 18 | 19 | catkin_package( 20 | CATKIN_DEPENDS actionlib_msgs message_runtime nav_2d_msgs 21 | ) 22 | 23 | -------------------------------------------------------------------------------- /locomotor_msgs/README.md: -------------------------------------------------------------------------------- 1 | # locomotor_msgs 2 | 3 | This class provides the `NavigateToPose` action for use with `actionlib+locomotor`. The idea is to provide more useful feedback than [MoveBase.action](http://docs.ros.org/api/move_base_msgs/html/action/MoveBase.html). 4 | 5 | Developers are encouraged to define their own actions that have statistics/constraints relative to their own domains. 6 | 7 | ## Progress City 8 | The `percent_complete` field in the `NavigateToPoseFeedback` is provided as a convenience, and is equivalent to `distance_traveled / (distance_traveled + estimated_distance_remaining)`. Note that with the current implementation, it is possible for `percent_complete` to go down if the `estimated_distance_remaining` goes up, which can happen when the global plan is blocked and a new longer global plan is found instead. 9 | 10 | ## I Want Results! 11 | When the action finishes, the action client can figure out whether the action succeeded by checking if the `state` is `SUCCEEDED` or `ABORTED`. The `SimpleActionServer/Client` also provides ["an optional text message"](https://github.com/ros/actionlib/blob/9210d811d105eabe72ff4741dece801f36e9064a/include/actionlib/server/simple_action_server.h#L165). While these two fields may provide sufficient information about the final state of the action, we provide one other method for providing final feedback. 12 | 13 | The `NavigateToPoseResult` contains a `ResultCode` message, which contains the integer field `result_code` and some enums for possible values of `result_code`. 14 | 15 | 16 | These Exception Codes are Based on 17 | 18 | 1. Which of the four components (Global/Local Costmap, Global/Local Planner) the error originates from 19 | 2. The particular exception thrown, based on the exception hierarchy defined in `nav_core2/exceptions.h` 20 | 21 | These particular result codes are used in conjunction with the `StateMachine`s defined in the `locomotor` package, but other user-implemented `StateMachine`s could use different custom-error codes, or build onto the existing error codes, as some space is left between the existing enum values. 22 | -------------------------------------------------------------------------------- /locomotor_msgs/action/NavigateToPose.action: -------------------------------------------------------------------------------- 1 | nav_2d_msgs/Pose2DStamped goal 2 | --- 3 | ResultCode result_code 4 | --- 5 | NavigationState state 6 | float32 percent_complete 7 | float32 distance_traveled 8 | float32 estimated_distance_remaining 9 | -------------------------------------------------------------------------------- /locomotor_msgs/msg/NavigationState.msg: -------------------------------------------------------------------------------- 1 | nav_2d_msgs/Pose2DStamped global_pose 2 | nav_2d_msgs/Pose2DStamped local_pose 3 | nav_2d_msgs/Pose2DStamped goal 4 | nav_2d_msgs/Twist2DStamped current_velocity 5 | nav_2d_msgs/Twist2DStamped command_velocity 6 | nav_2d_msgs/Path2D global_plan 7 | -------------------------------------------------------------------------------- /locomotor_msgs/msg/ResultCode.msg: -------------------------------------------------------------------------------- 1 | # This message contains three separate pieces. 2 | # A) A code indicating which component(s) the error originates from (bitmask style) 3 | # B) A code corresponding with the result_code defined in nav_core2/s.h 4 | # C) A freeform string message 5 | 6 | # The enumerations below are not necessarily the exclusive values for the codes. 7 | # Others may implement additional values beyond the ones shown, using custom state machines. 8 | 9 | ########### Component Values ############################################### 10 | int32 GLOBAL_COSTMAP = 1 11 | int32 LOCAL_COSTMAP = 2 12 | int32 GLOBAL_PLANNER = 4 13 | int32 LOCAL_PLANNER = 8 14 | 15 | ########### Result Codes ################################################### 16 | int32 GENERIC_COSTMAP=0 17 | int32 COSTMAP_SAFETY=1 18 | int32 COSTMAP_DATA_LAG=2 19 | int32 GENERIC_PLANNER=3 20 | int32 GENERIC_GLOBAL_PLANNER=4 21 | int32 INVALID_START=5 22 | int32 START_BOUNDS=6 23 | int32 OCCUPIED_START=7 24 | int32 INVALID_GOAL=8 25 | int32 GOAL_BOUNDS=9 26 | int32 OCCUPIED_GOAL=10 27 | int32 NO_GLOBAL_PATH=11 28 | int32 GLOBAL_PLANNER_TIMEOUT=12 29 | int32 GENERIC_LOCAL_PLANNER=13 30 | int32 ILLEGAL_TRAJECTORY=14 31 | int32 NO_LEGAL_TRAJECTORIES=15 32 | int32 PLANNER_TF=16 33 | 34 | ########### Actual Data #################################################### 35 | int32 component 36 | int32 result_code 37 | string message 38 | -------------------------------------------------------------------------------- /locomotor_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | locomotor_msgs 4 | 0.3.1 5 | Action definition for Locomotor 6 | 7 | David V. Lu!! 8 | 9 | BSD 10 | 11 | catkin 12 | actionlib_msgs 13 | nav_2d_msgs 14 | message_generation 15 | message_runtime 16 | message_runtime 17 | 18 | 19 | -------------------------------------------------------------------------------- /locomove_base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(locomove_base) 3 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror") 4 | if(NOT "${CMAKE_CXX_STANDARD}") 5 | set(CMAKE_CXX_STANDARD 14) 6 | endif() 7 | 8 | find_package(catkin REQUIRED 9 | COMPONENTS locomotor move_base_msgs nav_2d_utils nav_core nav_core_adapter 10 | ) 11 | 12 | catkin_package( 13 | CATKIN_DEPENDS locomotor move_base_msgs nav_2d_utils nav_core nav_core_adapter 14 | INCLUDE_DIRS include 15 | ) 16 | 17 | add_executable(move_base src/locomove_base.cpp) 18 | add_dependencies(move_base ${catkin_EXPORTED_TARGETS}) 19 | target_link_libraries(move_base ${catkin_LIBRARIES}) 20 | include_directories( 21 | include ${catkin_INCLUDE_DIRS} 22 | ) 23 | 24 | if(CATKIN_ENABLE_TESTING) 25 | find_package(roslint REQUIRED) 26 | roslint_cpp() 27 | roslint_add_test() 28 | endif() 29 | 30 | install( 31 | TARGETS move_base 32 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 33 | ) 34 | install( 35 | DIRECTORY include/${PROJECT_NAME}/ 36 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 37 | ) 38 | -------------------------------------------------------------------------------- /locomove_base/doc/state_machine.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/locomove_base/doc/state_machine.png -------------------------------------------------------------------------------- /locomove_base/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | locomove_base 4 | 0.3.1 5 | Extension of locomotor that implements move_base's functionality. 6 | David V. Lu!! 7 | BSD 8 | catkin 9 | locomotor 10 | move_base_msgs 11 | nav_2d_utils 12 | nav_core 13 | nav_core_adapter 14 | roslint 15 | 16 | -------------------------------------------------------------------------------- /nav_2d_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(nav_2d_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | message_generation 7 | std_msgs 8 | ) 9 | 10 | add_message_files(FILES 11 | ComplexPolygon2D.msg 12 | NavGridInfo.msg 13 | NavGridOfChars.msg 14 | NavGridOfCharsUpdate.msg 15 | NavGridOfDoubles.msg 16 | NavGridOfDoublesUpdate.msg 17 | Path2D.msg 18 | Point2D.msg 19 | Polygon2D.msg 20 | Polygon2DCollection.msg 21 | Polygon2DStamped.msg 22 | Pose2D32.msg 23 | Pose2DStamped.msg 24 | Twist2D.msg 25 | Twist2D32.msg 26 | Twist2DStamped.msg 27 | UIntBounds.msg 28 | ) 29 | 30 | add_service_files(FILES SwitchPlugin.srv) 31 | 32 | generate_messages(DEPENDENCIES geometry_msgs std_msgs) 33 | 34 | catkin_package( 35 | CATKIN_DEPENDS geometry_msgs message_runtime std_msgs 36 | ) 37 | -------------------------------------------------------------------------------- /nav_2d_msgs/README.md: -------------------------------------------------------------------------------- 1 | # nav_2d_msgs 2 | 3 | This package has basic message types for two dimensional navigation. Many of the messages are similar to those in `geometry_msgs` or `nav_msgs` but are streamlined to only be concerned with 2.5 dimensional `(x, y, theta)` navigation. This eliminates quaternions from the messages in many places, which make calculations faster and avoids that particular headache when only one orientation value is needed for 2.5D navigation. 4 | 5 | ## Points and Poses 6 | * [`Point2D`](msg/Point2D.msg) - like `geometry_msgs::Point` but just `x` and `y` (no `z`) 7 | * [`Pose2DStamped`](msg/Pose2DStamped.msg) - `geometry_msgs::Pose2D` with a header 8 | * [`Pose2D32`](msg/Pose2D32.msg) - `(x, y, theta)` with only 32bit floating point precision. 9 | * [`Path2D`](msg/Path2D.msg) - An array of `Pose2D` with a header. Similar to `nav_msgs::Path` but without redundant headers. 10 | 11 | ## Polygons 12 | * [`Polygon2D`](msg/Polygon2D.msg) - Like `geometry_msgs::Polygon` but with 64 bit precision and no `z` coordinate. 13 | * [`Polygon2DStamped`](msg/Polygon2DStamped.msg) - above with a header 14 | * [`ComplexPolygon2D`](msg/ComplexPolygon2D.msg) - Non-simple Polygon2D, i.e. polygon with inner holes 15 | * [`Polygon2DCollection`](msg/Polygon2DCollection.msg) - A list of complex polygons, with a header and an optional parallel list of colors. 16 | 17 | ## Twists 18 | * [`Twist2D`](msg/Twist2D.msg) - Like `geometry_msgs::Twist` but only `(x, y, theta)` (and not separated into linear and angular) 19 | * [`Twist2DStamped`](msg/Twist2DStamped.msg) - above with a header 20 | * [`Twist2D32`](msg/Twist2D32.msg) - `(x, y, theta)` with only 32bit floating point precision. 21 | 22 | ## NavGrids 23 | * [`NavGridInfo`](msg/NavGridInfo.msg) - Same data as `nav_grid::NavGridInfo`. Similar to `nav_msgs::MapMetadata` 24 | * [`NavGridOfChars`](msg/NavGridOfChars.msg) - Data for `nav_grid::NavGrid`. Similar to `nav_msgs::OccupancyGrid` 25 | * [`NavGridOfDoubles`](msg/NavGridOfDoubles.msg) - Data for `nav_grid::NavGrid` 26 | * [`NavGridOfCharsUpdate`](msg/NavGridOfCharsUpdate.msg) and [`NavGridOfDoublesUpdate`](msg/NavGridOfDoublesUpdate.msg) - Similar to `map_msgs::OccupancyGridUpdate` 27 | * [`UIntBounds`](msg/UIntBounds.msg) - Same data as `nav_core2::UIntBounds`. Used in both `Update` messages. 28 | 29 | ## Service 30 | * [`SwitchPlugin`](srv/SwitchPlugin.srv) - A simple service equivalent to [SetString.srv](https://discourse.ros.org/t/suggestions-for-std-srvs/1079) use by the PluginMux. -------------------------------------------------------------------------------- /nav_2d_msgs/msg/ComplexPolygon2D.msg: -------------------------------------------------------------------------------- 1 | # Representation for a non-simple polygon, i.e. one with holes 2 | Polygon2D outer # The outer perimeter 3 | Polygon2D[] inner # The perimeter of any inner holes 4 | -------------------------------------------------------------------------------- /nav_2d_msgs/msg/NavGridInfo.msg: -------------------------------------------------------------------------------- 1 | uint32 width 2 | uint32 height 3 | float64 resolution 4 | string frame_id 5 | float64 origin_x 6 | float64 origin_y 7 | -------------------------------------------------------------------------------- /nav_2d_msgs/msg/NavGridOfChars.msg: -------------------------------------------------------------------------------- 1 | time stamp 2 | NavGridInfo info 3 | # The map data, in row-major order, starting with (0,0). 4 | # Unlike nav_msgs/OccupancyGrid, the values are [0, 256), not [-1, 100] 5 | uint8[] data 6 | -------------------------------------------------------------------------------- /nav_2d_msgs/msg/NavGridOfCharsUpdate.msg: -------------------------------------------------------------------------------- 1 | time stamp 2 | UIntBounds bounds 3 | uint8[] data 4 | -------------------------------------------------------------------------------- /nav_2d_msgs/msg/NavGridOfDoubles.msg: -------------------------------------------------------------------------------- 1 | time stamp 2 | NavGridInfo info 3 | # The map data, in row-major order, starting with (0,0). 4 | float64[] data 5 | -------------------------------------------------------------------------------- /nav_2d_msgs/msg/NavGridOfDoublesUpdate.msg: -------------------------------------------------------------------------------- 1 | time stamp 2 | UIntBounds bounds 3 | float64[] data 4 | -------------------------------------------------------------------------------- /nav_2d_msgs/msg/Path2D.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Pose2D[] poses 3 | -------------------------------------------------------------------------------- /nav_2d_msgs/msg/Point2D.msg: -------------------------------------------------------------------------------- 1 | float64 x 2 | float64 y 3 | -------------------------------------------------------------------------------- /nav_2d_msgs/msg/Polygon2D.msg: -------------------------------------------------------------------------------- 1 | Point2D[] points 2 | -------------------------------------------------------------------------------- /nav_2d_msgs/msg/Polygon2DCollection.msg: -------------------------------------------------------------------------------- 1 | # Primarily used for visualization 2 | # Colors are optional 3 | std_msgs/Header header 4 | ComplexPolygon2D[] polygons 5 | std_msgs/ColorRGBA[] colors 6 | -------------------------------------------------------------------------------- /nav_2d_msgs/msg/Polygon2DStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | Polygon2D polygon 3 | -------------------------------------------------------------------------------- /nav_2d_msgs/msg/Pose2D32.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 theta 4 | -------------------------------------------------------------------------------- /nav_2d_msgs/msg/Pose2DStamped.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Pose2D pose 3 | -------------------------------------------------------------------------------- /nav_2d_msgs/msg/Twist2D.msg: -------------------------------------------------------------------------------- 1 | float64 x 2 | float64 y 3 | float64 theta 4 | -------------------------------------------------------------------------------- /nav_2d_msgs/msg/Twist2D32.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 theta 4 | -------------------------------------------------------------------------------- /nav_2d_msgs/msg/Twist2DStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | Twist2D velocity 3 | -------------------------------------------------------------------------------- /nav_2d_msgs/msg/UIntBounds.msg: -------------------------------------------------------------------------------- 1 | # Bounds are inclusive 2 | uint32 min_x 3 | uint32 min_y 4 | uint32 max_x 5 | uint32 max_y 6 | -------------------------------------------------------------------------------- /nav_2d_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nav_2d_msgs 4 | 0.3.1 5 | Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. 6 | David V. Lu!! 7 | BSD 8 | 9 | catkin 10 | geometry_msgs 11 | std_msgs 12 | message_generation 13 | message_runtime 14 | message_runtime 15 | 16 | 17 | -------------------------------------------------------------------------------- /nav_2d_msgs/srv/SwitchPlugin.srv: -------------------------------------------------------------------------------- 1 | string new_plugin 2 | --- 3 | bool success 4 | string message 5 | -------------------------------------------------------------------------------- /nav_2d_utils/README.md: -------------------------------------------------------------------------------- 1 | # nav_2d_utils 2 | A handful of useful utility functions for nav_core2 packages. 3 | * Bounds - Utilities for `nav_core2::Bounds` objects interacting with other messages/types and for dividing a `UIntBounds` into multiple smaller bounds. 4 | * [Conversions](doc/Conversions.md) - Tools for converting between `nav_2d_msgs` and other types. 5 | * OdomSubscriber - subscribes to the standard `nav_msgs::Odometry` message and provides access to the velocity component as a `nav_2d_msgs::Twist` 6 | * Parameters - a couple ROS parameter patterns 7 | * PathOps - functions for working with `nav_2d_msgs::Path2D` objects (beyond strict conversion) 8 | * [Plugin Mux](doc/PluginMux.md) - tool for switching between multiple `pluginlib` plugins 9 | * [Polygons and Footprints](doc/PolygonsAndFootprints.md) - functions for working with `Polygon2D` objects 10 | * TF Help - Tools for transforming `nav_2d_msgs` and other common operations. 11 | -------------------------------------------------------------------------------- /nav_2d_utils/doc/Conversions.md: -------------------------------------------------------------------------------- 1 | # nav_2d_utils Conversions 2 | 3 | (note: exact syntax differs from below for conciseness, leaving out `const` and `&`) 4 | 5 | ## Twist Transformations 6 | | to `nav_2d_msgs` | from `nav_2d_msgs` | 7 | | -- | -- | 8 | | `Twist2D twist3Dto2D(geometry_msgs::Twist)` | `geometry_msgs::Twist twist2Dto3D(Twist2D cmd_vel_2d)` 9 | 10 | ## Point Transformations 11 | | to `nav_2d_msgs` | from `nav_2d_msgs` | 12 | | -- | -- | 13 | | `Point2D pointToPoint2D(geometry_msgs::Point)` | `geometry_msgs::Point pointToPoint3D(Point2D)` 14 | | `Point2D pointToPoint2D(geometry_msgs::Point32)` | `geometry_msgs::Point32 pointToPoint32(Point2D)` 15 | 16 | ## Pose Transformations 17 | | to `nav_2d_msgs` | from `nav_2d_msgs` | 18 | | -- | -- | 19 | | `Pose2DStamped poseStampedToPose2D(geometry_msgs::PoseStamped)` | `geometry_msgs::PoseStamped pose2DToPoseStamped(Pose2DStamped)` 20 | ||`geometry_msgs::PoseStamped pose2DToPoseStamped(geometry_msgs::Pose2D, std::string, ros::Time)`| 21 | ||`geometry_msgs::Pose pose2DToPose(geometry_msgs::Pose2D)`| 22 | | `Pose2DStamped stampedPoseToPose2D(tf::Stamped)` | | 23 | 24 | ## Path Transformations 25 | | to `nav_2d_msgs` | from `nav_2d_msgs` | 26 | | -- | -- | 27 | | `Path2D pathToPath(nav_msgs::Path)` |`nav_msgs::Path pathToPath(Path2D)` 28 | | `Path2D posesToPath2D(std::vector)` | `nav_msgs::Path poses2DToPath(std::vector, std::string, ros::Time)` 29 | 30 | Also, `nav_msgs::Path posesToPath(std::vector)` 31 | 32 | ## Polygon Transformations 33 | | to `nav_2d_msgs` | from `nav_2d_msgs` | 34 | | -- | -- | 35 | | `Polygon2D polygon3Dto2D(geometry_msgs::Polygon)` |`geometry_msgs::Polygon polygon2Dto3D(Polygon2D)` 36 | | `Polygon2DStamped polygon3Dto2D(geometry_msgs::PolygonStamped)` | `geometry_msgs::PolygonStamped polygon2Dto3D(Polygon2DStamped)` 37 | 38 | ## Info Transformations 39 | | to `nav_2d_msgs` | from `nav_2d_msgs` | 40 | | -- | -- | 41 | |`nav_2d_msgs::NavGridInfo toMsg(nav_grid::NavGridInfo)`|`nav_grid::NavGridInfo fromMsg(nav_2d_msgs::NavGridInfo)`| 42 | 43 | | to `nav_grid` info | from `nav_grid` info | 44 | | -- | -- | 45 | |`nav_grid::NavGridInfo infoToInfo(nav_msgs::MapMetaData, std::string)` | `nav_msgs::MapMetaData infoToInfo(nav_grid::NavGridInfo)` 46 | 47 | | to two dimensional pose | to three dimensional pose | 48 | | -- | -- | 49 | | `Pose2D getOrigin2D(nav_grid::NavGridInfo)` | `geometry_msgs::Pose getOrigin3D(nav_grid::NavGridInfo)`| 50 | 51 | ## Bounds Transformations 52 | | to `nav_2d_msgs` | from `nav_2d_msgs` | 53 | | -- | -- | 54 | |`nav_2d_msgs::UIntBounds toMsg(nav_core2::UIntBounds)`|`nav_core2::UIntBounds fromMsg(nav_2d_msgs::UIntBounds)`| 55 | -------------------------------------------------------------------------------- /nav_2d_utils/doc/PluginMux.md: -------------------------------------------------------------------------------- 1 | 2 | # Plugin Mux 3 | `PluginMux` is an organizer for switching between multiple different plugins of the same type. It may be easiest to see how it operates through an example. Let's say we have multiple global planners we would like to use at different times, with only one being active at a given time. 4 | 5 | This means we have multiple `pluginlib` plugins to load that extend the `BaseGlobalPlanner` interface from the `nav_core` package. We define multiple namespaces in the `global_planner_namespaces` and load each of them. Here's an example parameter config. 6 | 7 | ``` 8 | global_planner_namespaces: 9 | - boring_nav_fn 10 | - wacky_global_planner 11 | boring_nav_fn: 12 | allow_unknown: true 13 | plugin_class: navfn/NavfnROS 14 | wacky_global_planner: 15 | allow_unknown: false 16 | # default value commented out 17 | # plugin_class: global_planner/GlobalPlanner 18 | ``` 19 | 20 | The namespaces are arbitrary strings, and need not reflect the name of the planner. The package and class name for the plugin will be specified by the `plugin_class` parameter. By default, the first namespace will be loaded as the current plugin. 21 | 22 | To advertise which plugin is active, we publish the namespace on a latched topic and set a parameter with the same name (`~/current_global_planner`). We can then switch among them with a `SetString` ROS service call, or the `usePlugin` C++ method. 23 | 24 | This configuration is all created by creating a plugin mux with the following parameters (parameter names shown for convenience): 25 | ``` 26 | PluginMux(plugin_package = "nav_core", 27 | plugin_class = "BaseGlobalPlanner", 28 | parameter_name = "global_planner_namespaces", 29 | default_value = "global_planner/GlobalPlanner", 30 | ros_name = "current_global_planner", 31 | switch_service_name = "switch_global_planner"); 32 | ``` 33 | 34 | If the parameter is not set or is an empty list, a namespace will be derived from the `default_value` and be loaded as the only plugin available. 35 | ``` 36 | global_planner_namespaces: [] 37 | GlobalPlanner: 38 | allow_unknown: true 39 | ``` 40 | -------------------------------------------------------------------------------- /nav_2d_utils/include/mapbox/LICENSE: -------------------------------------------------------------------------------- 1 | ISC License 2 | 3 | Copyright (c) 2015, Mapbox 4 | 5 | Permission to use, copy, modify, and/or distribute this software for any purpose 6 | with or without fee is hereby granted, provided that the above copyright notice 7 | and this permission notice appear in all copies. 8 | 9 | THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH 10 | REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND 11 | FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, 12 | INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS 13 | OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER 14 | TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF 15 | THIS SOFTWARE. 16 | -------------------------------------------------------------------------------- /nav_2d_utils/include/mapbox/NOTES: -------------------------------------------------------------------------------- 1 | A C++ port of earcut.js, a fast, header-only polygon triangulation library. 2 | https://github.com/mapbox/earcut.hpp 3 | -------------------------------------------------------------------------------- /nav_2d_utils/include/nav_2d_utils/footprint.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 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 copyright holder 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 HOLDER 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 | #ifndef NAV_2D_UTILS_FOOTPRINT_H 36 | #define NAV_2D_UTILS_FOOTPRINT_H 37 | 38 | #include 39 | #include 40 | 41 | namespace nav_2d_utils 42 | { 43 | 44 | /** 45 | * @brief Load the robot footprint either as a polygon from the footprint parameter or from robot_radius 46 | * 47 | * Analagous to costmap_2d::makeFootprintFromParams in that it will return an empty polygon if neither parameter 48 | * is present. 49 | */ 50 | nav_2d_msgs::Polygon2D footprintFromParams(ros::NodeHandle& nh, bool write = true); 51 | 52 | } // namespace nav_2d_utils 53 | 54 | #endif // NAV_2D_UTILS_FOOTPRINT_H 55 | -------------------------------------------------------------------------------- /nav_2d_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nav_2d_utils 4 | 0.3.1 5 | A handful of useful utility functions for nav_core2 packages. 6 | 7 | David V. Lu!! 8 | 9 | BSD 10 | 11 | catkin 12 | python3-setuptools 13 | 14 | geometry_msgs 15 | nav_2d_msgs 16 | nav_core2 17 | nav_grid 18 | nav_msgs 19 | pluginlib 20 | roscpp 21 | std_msgs 22 | tf 23 | tf2_ros 24 | tf2_geometry_msgs 25 | xmlrpcpp 26 | roslint 27 | rostest 28 | rosunit 29 | 30 | -------------------------------------------------------------------------------- /nav_2d_utils/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | package_info = generate_distutils_setup( 7 | packages=['nav_2d_utils'], 8 | package_dir={'': 'src'} 9 | ) 10 | 11 | setup(**package_info) 12 | -------------------------------------------------------------------------------- /nav_2d_utils/src/footprint.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 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 copyright holder 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 HOLDER 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 | #include 37 | #include 38 | 39 | namespace nav_2d_utils 40 | { 41 | nav_2d_msgs::Polygon2D footprintFromParams(ros::NodeHandle& nh, bool write) 42 | { 43 | std::string full_param_name; 44 | nav_2d_msgs::Polygon2D footprint; 45 | 46 | if (nh.searchParam("footprint", full_param_name)) 47 | { 48 | footprint = polygonFromParams(nh, full_param_name, false); 49 | if (write) 50 | { 51 | nh.setParam("footprint", polygonToXMLRPC(footprint)); 52 | } 53 | } 54 | else if (nh.searchParam("robot_radius", full_param_name)) 55 | { 56 | double robot_radius = 0.0; 57 | nh.getParam(full_param_name, robot_radius); 58 | footprint = polygonFromRadius(robot_radius); 59 | if (write) 60 | { 61 | nh.setParam("robot_radius", robot_radius); 62 | } 63 | } 64 | return footprint; 65 | } 66 | 67 | } // namespace nav_2d_utils 68 | -------------------------------------------------------------------------------- /nav_2d_utils/src/nav_2d_utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_2d_utils/src/nav_2d_utils/__init__.py -------------------------------------------------------------------------------- /nav_2d_utils/test/param_tests.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | footprint: [[0.1, 0.1], [-0.1, 0.1], [-0.1, -0.1], [0.1, -0.1]] 7 | footprint2: 8 | x: [0.1, -0.1, -0.1, 0.1] 9 | y: [0.1, 0.1, -0.1, -0.1] 10 | 11 | 12 | 13 | footprint: [[0.1, 0.1], [-0.1, 0.1, 77.0], [-0.1, -0.1], [0.1, -0.1]] 14 | footprint2: 1.0 15 | footprint3: false 16 | footprint4: [[0.1, 0.1], [0.1, 0.1]] 17 | footprint5: ['x', 'y', 'z'] 18 | footprint6: [['x', 'y'], ['a', 'b'], ['c'], ['d']] 19 | footprint7: 20 | x: [0.1, -0.1, -0.1, 0.1] 21 | footprint8: 22 | x: 0 23 | y: 0 24 | footprint9: 25 | x: ['a', 'b', 'c'] 26 | y: [d, e, f] 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /nav_core2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(nav_core2) 3 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11") 4 | 5 | find_package(catkin REQUIRED 6 | COMPONENTS nav_2d_msgs nav_grid tf2_ros 7 | ) 8 | 9 | catkin_package( 10 | CATKIN_DEPENDS nav_2d_msgs nav_grid tf2_ros 11 | INCLUDE_DIRS include 12 | LIBRARIES basic_costmap 13 | ) 14 | 15 | add_library(basic_costmap src/basic_costmap.cpp) 16 | target_link_libraries( 17 | basic_costmap ${catkin_LIBRARIES} 18 | ) 19 | 20 | include_directories( 21 | include ${catkin_INCLUDE_DIRS} 22 | ) 23 | 24 | if(CATKIN_ENABLE_TESTING) 25 | find_package(roslint REQUIRED) 26 | roslint_cpp() 27 | roslint_add_test() 28 | 29 | catkin_add_gtest(bounds_test test/bounds_test.cpp) 30 | catkin_add_gtest(exception_test test/exception_test.cpp) 31 | endif() 32 | 33 | install(DIRECTORY include/${PROJECT_NAME}/ 34 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 35 | ) 36 | install( 37 | TARGETS basic_costmap 38 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 39 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 40 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 41 | ) 42 | -------------------------------------------------------------------------------- /nav_core2/doc/exceptions.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_core2/doc/exceptions.png -------------------------------------------------------------------------------- /nav_core2/include/nav_core2/basic_costmap.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 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 copyright holder 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 HOLDER 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 | #ifndef NAV_CORE2_BASIC_COSTMAP_H 36 | #define NAV_CORE2_BASIC_COSTMAP_H 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | namespace nav_core2 43 | { 44 | class BasicCostmap : public nav_core2::Costmap 45 | { 46 | public: 47 | // Standard Costmap Interface 48 | mutex_t* getMutex() override { return &my_mutex_; } 49 | 50 | // NavGrid Interface 51 | void reset() override; 52 | void setValue(const unsigned int x, const unsigned int y, const unsigned char& value) override; 53 | unsigned char getValue(const unsigned int x, const unsigned int y) const override; 54 | void setInfo(const nav_grid::NavGridInfo& new_info) override 55 | { 56 | info_ = new_info; 57 | reset(); 58 | } 59 | 60 | // Index Conversion 61 | unsigned int getIndex(const unsigned int x, const unsigned int y) const; 62 | protected: 63 | mutex_t my_mutex_; 64 | std::vector data_; 65 | }; 66 | } // namespace nav_core2 67 | 68 | #endif // NAV_CORE2_BASIC_COSTMAP_H 69 | -------------------------------------------------------------------------------- /nav_core2/include/nav_core2/common.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Locus Robotics 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 copyright holder 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 HOLDER 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 NAV_CORE2_COMMON_H 35 | #define NAV_CORE2_COMMON_H 36 | 37 | #include 38 | #include 39 | 40 | using TFListenerPtr = std::shared_ptr; 41 | 42 | #endif // NAV_CORE2_COMMON_H 43 | -------------------------------------------------------------------------------- /nav_core2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nav_core2 4 | 0.3.1 5 | 6 | Interfaces for Costmap, LocalPlanner and GlobalPlanner. Replaces nav_core. 7 | 8 | David V. Lu!! 9 | BSD 10 | 11 | catkin 12 | nav_2d_msgs 13 | nav_grid 14 | tf2_ros 15 | roslint 16 | rosunit 17 | 18 | -------------------------------------------------------------------------------- /nav_core2/src/basic_costmap.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 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 copyright holder 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 HOLDER 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 nav_core2 38 | { 39 | 40 | void BasicCostmap::reset() 41 | { 42 | data_.assign(info_.width * info_.height, this->default_value_); 43 | } 44 | 45 | unsigned int BasicCostmap::getIndex(const unsigned int x, const unsigned int y) const 46 | { 47 | return y * info_.width + x; 48 | } 49 | 50 | unsigned char BasicCostmap::getValue(const unsigned int x, const unsigned int y) const 51 | { 52 | return data_[getIndex(x, y)]; 53 | } 54 | 55 | void BasicCostmap::setValue(const unsigned int x, const unsigned int y, const unsigned char& value) 56 | { 57 | data_[getIndex(x, y)] = value; 58 | } 59 | 60 | } // namespace nav_core2 61 | -------------------------------------------------------------------------------- /nav_core_adapter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(nav_core_adapter) 3 | 4 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror") 5 | if(NOT "${CMAKE_CXX_STANDARD}") 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | costmap_2d 11 | geometry_msgs 12 | nav_2d_msgs 13 | nav_2d_utils 14 | nav_core 15 | nav_core2 16 | nav_grid 17 | nav_msgs 18 | pluginlib 19 | tf 20 | visualization_msgs 21 | ) 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS 25 | costmap_2d 26 | geometry_msgs 27 | nav_2d_msgs 28 | nav_2d_utils 29 | nav_core 30 | nav_core2 31 | nav_grid 32 | nav_msgs 33 | pluginlib 34 | tf 35 | visualization_msgs 36 | INCLUDE_DIRS include 37 | LIBRARIES local_planner_adapter global_planner_adapter costmap_adapter global_planner_adapter2 38 | ) 39 | 40 | add_library(costmap_adapter src/costmap_adapter.cpp) 41 | target_link_libraries(costmap_adapter ${catkin_LIBRARIES}) 42 | 43 | add_library(local_planner_adapter src/local_planner_adapter.cpp) 44 | add_dependencies(local_planner_adapter ${catkin_EXPORTED_TARGETS}) 45 | target_link_libraries(local_planner_adapter costmap_adapter ${catkin_LIBRARIES}) 46 | 47 | add_library(global_planner_adapter src/global_planner_adapter.cpp) 48 | add_dependencies(global_planner_adapter ${catkin_EXPORTED_TARGETS}) 49 | target_link_libraries(global_planner_adapter costmap_adapter ${catkin_LIBRARIES}) 50 | 51 | add_library(global_planner_adapter2 src/global_planner_adapter2.cpp) 52 | add_dependencies(global_planner_adapter2 ${catkin_EXPORTED_TARGETS}) 53 | target_link_libraries(global_planner_adapter2 ${catkin_LIBRARIES}) 54 | 55 | include_directories( 56 | include ${catkin_INCLUDE_DIRS} 57 | ) 58 | 59 | if(CATKIN_ENABLE_TESTING) 60 | find_package(rostest REQUIRED) 61 | find_package(roslint REQUIRED) 62 | roslint_cpp() 63 | roslint_add_test() 64 | 65 | add_rostest_gtest(unload_test test/unload_test.launch test/unload_test.cpp) 66 | target_link_libraries(unload_test local_planner_adapter ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) 67 | endif() 68 | 69 | install(TARGETS local_planner_adapter global_planner_adapter costmap_adapter global_planner_adapter2 70 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 71 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 72 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 73 | ) 74 | install(DIRECTORY include/${PROJECT_NAME}/ 75 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 76 | ) 77 | install(FILES nav_core2_plugins.xml nav_core_plugins.xml 78 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 79 | ) 80 | -------------------------------------------------------------------------------- /nav_core_adapter/nav_core2_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /nav_core_adapter/nav_core_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /nav_core_adapter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nav_core_adapter 4 | 0.3.1 5 | 6 | This package contains adapters for using `nav_core` plugins as `nav_core2` plugins and vice versa (more or less). 7 | See README.md for more information. 8 | 9 | 10 | David V. Lu!! 11 | 12 | BSD 13 | 14 | catkin 15 | costmap_2d 16 | geometry_msgs 17 | nav_2d_msgs 18 | nav_2d_utils 19 | nav_core 20 | nav_core2 21 | nav_grid 22 | nav_msgs 23 | pluginlib 24 | tf 25 | visualization_msgs 26 | roslint 27 | rostest 28 | 29 | 30 | dwb_local_planner 31 | dwb_plugins 32 | dwb_critics 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /nav_core_adapter/test/unload_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /nav_grid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(nav_grid) 3 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11") 4 | 5 | find_package(catkin REQUIRED) 6 | 7 | catkin_package( 8 | INCLUDE_DIRS include 9 | ) 10 | 11 | if (CATKIN_ENABLE_TESTING) 12 | find_package(roslint REQUIRED) 13 | include_directories(include ${catkin_INCLUDE_DIRS}) 14 | roslint_cpp() 15 | roslint_add_test() 16 | catkin_add_gtest(${PROJECT_NAME}_utest test/utest.cpp) 17 | endif (CATKIN_ENABLE_TESTING) 18 | 19 | install(DIRECTORY include/${PROJECT_NAME}/ 20 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 21 | ) 22 | -------------------------------------------------------------------------------- /nav_grid/doc/change_info.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_grid/doc/change_info.png -------------------------------------------------------------------------------- /nav_grid/doc/coords.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_grid/doc/coords.png -------------------------------------------------------------------------------- /nav_grid/doc/nav_grid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_grid/doc/nav_grid.png -------------------------------------------------------------------------------- /nav_grid/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nav_grid 4 | 0.3.1 5 | 6 | A templatized interface for overlaying a two dimensional grid on the world. 7 | 8 | David V. Lu!! 9 | BSD 10 | catkin 11 | roslint 12 | rosunit 13 | 14 | -------------------------------------------------------------------------------- /nav_grid_iterators/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(nav_grid_iterators) 3 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror") 4 | if(NOT "${CMAKE_CXX_STANDARD}") 5 | set(CMAKE_CXX_STANDARD 14) 6 | endif() 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | nav_grid nav_2d_msgs nav_2d_utils nav_msgs roscpp 10 | ) 11 | 12 | catkin_package( 13 | CATKIN_DEPENDS nav_grid nav_2d_msgs nav_2d_utils nav_msgs roscpp 14 | INCLUDE_DIRS include 15 | LIBRARIES nav_grid_iterators 16 | ) 17 | 18 | add_library(nav_grid_iterators 19 | src/whole_grid.cpp 20 | src/sub_grid.cpp 21 | src/circle_fill.cpp 22 | src/circle_outline.cpp 23 | src/spiral.cpp 24 | src/bresenham.cpp 25 | src/ray_trace.cpp 26 | src/line.cpp 27 | src/polygon_outline.cpp 28 | src/polygon_fill.cpp 29 | ) 30 | 31 | target_link_libraries( 32 | nav_grid_iterators ${catkin_LIBRARIES} 33 | ) 34 | 35 | add_executable(demo demo/demo.cpp) 36 | add_dependencies(demo ${catkin_EXPORTED_TARGETS}) 37 | target_link_libraries(demo nav_grid_iterators ${catkin_LIBRARIES}) 38 | 39 | include_directories( 40 | include ${catkin_INCLUDE_DIRS} 41 | ) 42 | 43 | if (CATKIN_ENABLE_TESTING) 44 | find_package(roslint REQUIRED) 45 | roslint_cpp() 46 | roslint_add_test() 47 | 48 | catkin_add_gtest(line_tests test/line_tests.cpp) 49 | target_link_libraries(line_tests nav_grid_iterators) 50 | catkin_add_gtest(${PROJECT_NAME}_utest test/utest.cpp) 51 | target_link_libraries(${PROJECT_NAME}_utest nav_grid_iterators) 52 | endif (CATKIN_ENABLE_TESTING) 53 | 54 | install(TARGETS nav_grid_iterators 55 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 56 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 57 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 58 | ) 59 | install(DIRECTORY include/${PROJECT_NAME}/ 60 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 61 | ) 62 | -------------------------------------------------------------------------------- /nav_grid_iterators/demo/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /nav_grid_iterators/demo/demo.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_grid_iterators/demo/demo.mp4 -------------------------------------------------------------------------------- /nav_grid_iterators/demo/demo.rviz: -------------------------------------------------------------------------------- 1 | Visualization Manager: 2 | Displays: 3 | - Class: rviz/Map 4 | Color Scheme: costmap 5 | Enabled: true 6 | Name: Map 7 | Topic: /map 8 | Views: 9 | Current: 10 | Angle: 0 11 | Class: rviz/TopDownOrtho 12 | Name: Current View 13 | Scale: 100 14 | Value: TopDownOrtho (rviz) 15 | X: 3.5 16 | Y: 2.5 17 | Window Geometry: 18 | Height: 1400 19 | Width: 2500 20 | -------------------------------------------------------------------------------- /nav_grid_iterators/include/nav_grid_iterators/iterators.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 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 copyright holder 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 HOLDER 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 | #ifndef NAV_GRID_ITERATORS_ITERATORS_H 36 | #define NAV_GRID_ITERATORS_ITERATORS_H 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #endif // NAV_GRID_ITERATORS_ITERATORS_H 48 | -------------------------------------------------------------------------------- /nav_grid_iterators/include/nav_grid_iterators/whole_grid.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 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 copyright holder 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 HOLDER 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 | #ifndef NAV_GRID_ITERATORS_WHOLE_GRID_H 36 | #define NAV_GRID_ITERATORS_WHOLE_GRID_H 37 | 38 | #include 39 | 40 | namespace nav_grid_iterators 41 | { 42 | /** 43 | * @class WholeGrid 44 | * An iterator that will iterate through all the cells of a grid in row-major order 45 | */ 46 | class WholeGrid : public BaseIterator 47 | { 48 | public: 49 | using BaseIterator::BaseIterator; 50 | 51 | /**@name Standard BaseIterator Interface */ 52 | /**@{*/ 53 | WholeGrid begin() const override; 54 | WholeGrid end() const override; 55 | void increment() override; 56 | /**@}*/ 57 | }; 58 | } // namespace nav_grid_iterators 59 | 60 | #endif // NAV_GRID_ITERATORS_WHOLE_GRID_H 61 | -------------------------------------------------------------------------------- /nav_grid_iterators/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nav_grid_iterators 4 | 0.3.1 5 | 6 | Iterator implementations for moving around the cells of a nav_grid in a number of common patterns. 7 | 8 | 9 | David V. Lu!! 10 | 11 | BSD 12 | 13 | catkin 14 | nav_2d_msgs 15 | nav_2d_utils 16 | nav_grid 17 | nav_msgs 18 | roscpp 19 | roslint 20 | rosunit 21 | 22 | -------------------------------------------------------------------------------- /nav_grid_iterators/src/whole_grid.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 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 copyright holder 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 HOLDER 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 nav_grid_iterators 38 | { 39 | 40 | WholeGrid WholeGrid::begin() const 41 | { 42 | return WholeGrid(info_); 43 | } 44 | 45 | WholeGrid WholeGrid::end() const 46 | { 47 | return WholeGrid(info_, nav_grid::Index(0, info_->height)); 48 | } 49 | 50 | void WholeGrid::increment() 51 | { 52 | ++index_.x; 53 | if (index_.x >= info_->width) 54 | { 55 | index_.x = 0; 56 | ++index_.y; 57 | } 58 | } 59 | 60 | } // namespace nav_grid_iterators 61 | -------------------------------------------------------------------------------- /nav_grid_pub_sub/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(nav_grid_pub_sub) 3 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror") 4 | if(NOT "${CMAKE_CXX_STANDARD}") 5 | set(CMAKE_CXX_STANDARD 14) 6 | endif() 7 | 8 | find_package(catkin REQUIRED 9 | COMPONENTS 10 | geometry_msgs 11 | map_msgs 12 | nav_2d_msgs 13 | nav_2d_utils 14 | nav_core2 15 | nav_grid 16 | nav_grid_iterators 17 | nav_msgs 18 | roscpp 19 | ) 20 | 21 | catkin_package( 22 | CATKIN_DEPENDS 23 | geometry_msgs 24 | map_msgs 25 | nav_2d_msgs 26 | nav_2d_utils 27 | nav_core2 28 | nav_grid 29 | nav_grid_iterators 30 | nav_msgs 31 | roscpp 32 | INCLUDE_DIRS include 33 | LIBRARIES nav_grid_pub_sub 34 | ) 35 | include_directories( 36 | include ${catkin_INCLUDE_DIRS} 37 | ) 38 | 39 | add_library(nav_grid_pub_sub src/cost_interpretation_tables.cpp) 40 | target_link_libraries(nav_grid_pub_sub ${catkin_LIBRARIES}) 41 | add_dependencies(nav_grid_pub_sub ${catkin_EXPORTED_TARGETS}) 42 | 43 | if(CATKIN_ENABLE_TESTING) 44 | find_package(roslint REQUIRED) 45 | roslint_cpp() 46 | roslint_add_test() 47 | endif() 48 | 49 | install( 50 | TARGETS nav_grid_pub_sub 51 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 52 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 53 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 54 | ) 55 | install( 56 | DIRECTORY include/${PROJECT_NAME}/ 57 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 58 | ) 59 | -------------------------------------------------------------------------------- /nav_grid_pub_sub/doc/input0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_grid_pub_sub/doc/input0.png -------------------------------------------------------------------------------- /nav_grid_pub_sub/doc/input1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_grid_pub_sub/doc/input1.png -------------------------------------------------------------------------------- /nav_grid_pub_sub/doc/negate.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_grid_pub_sub/doc/negate.png -------------------------------------------------------------------------------- /nav_grid_pub_sub/doc/publish.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_grid_pub_sub/doc/publish.png -------------------------------------------------------------------------------- /nav_grid_pub_sub/doc/raw.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_grid_pub_sub/doc/raw.png -------------------------------------------------------------------------------- /nav_grid_pub_sub/doc/scale_output.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_grid_pub_sub/doc/scale_output.png -------------------------------------------------------------------------------- /nav_grid_pub_sub/doc/scale_pixel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_grid_pub_sub/doc/scale_pixel.png -------------------------------------------------------------------------------- /nav_grid_pub_sub/doc/trinary_input.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_grid_pub_sub/doc/trinary_input.png -------------------------------------------------------------------------------- /nav_grid_pub_sub/doc/trinary_output.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_grid_pub_sub/doc/trinary_output.png -------------------------------------------------------------------------------- /nav_grid_pub_sub/doc/trinary_pixel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_grid_pub_sub/doc/trinary_pixel.png -------------------------------------------------------------------------------- /nav_grid_pub_sub/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nav_grid_pub_sub 4 | 0.3.1 5 | Publishers and Subscribers for nav_grid data. 6 | 7 | David V. Lu!! 8 | 9 | BSD 10 | 11 | catkin 12 | geometry_msgs 13 | map_msgs 14 | nav_2d_msgs 15 | nav_2d_utils 16 | nav_core2 17 | nav_grid 18 | nav_grid_iterators 19 | nav_msgs 20 | roscpp 21 | roslint 22 | 23 | 24 | -------------------------------------------------------------------------------- /nav_grid_server/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(nav_grid_server) 3 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror") 4 | if(NOT "${CMAKE_CXX_STANDARD}") 5 | set(CMAKE_CXX_STANDARD 14) 6 | endif() 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | nav_2d_utils 10 | nav_grid 11 | nav_grid_iterators 12 | nav_grid_pub_sub 13 | nav_msgs 14 | roscpp 15 | ) 16 | find_package(OpenCV REQUIRED) 17 | find_package(PkgConfig REQUIRED) 18 | pkg_check_modules(YAMLCPP yaml-cpp REQUIRED) 19 | 20 | catkin_package( 21 | DEPENDS OpenCV 22 | CATKIN_DEPENDS nav_2d_utils nav_grid nav_grid_iterators nav_grid_pub_sub nav_msgs roscpp 23 | INCLUDE_DIRS include 24 | LIBRARIES image_loader 25 | ) 26 | 27 | include_directories( 28 | include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${YAMLCPP_INCLUDE_DIRS} 29 | ) 30 | 31 | add_library(image_loader src/image_loader.cpp) 32 | target_link_libraries(image_loader ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) 33 | 34 | add_executable(server src/server.cpp) 35 | target_link_libraries(server image_loader ${catkin_LIBRARIES} 36 | ${YAMLCPP_LIBRARIES}) 37 | 38 | add_executable(saver src/saver.cpp) 39 | target_link_libraries(saver ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) 40 | 41 | if (CATKIN_ENABLE_TESTING) 42 | find_package(roslint REQUIRED) 43 | find_package(roslib REQUIRED) 44 | find_package(map_server REQUIRED) 45 | catkin_add_gtest(map_server_comparison test/map_server_comparison.cpp) 46 | target_link_libraries(map_server_comparison image_loader ${catkin_LIBRARIES} ${roslib_LIBRARIES} ${map_server_LIBRARIES}) 47 | 48 | roslint_cpp() 49 | roslint_add_test() 50 | endif() 51 | 52 | install( 53 | DIRECTORY include/${PROJECT_NAME}/ 54 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 55 | ) 56 | install( 57 | TARGETS image_loader 58 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 59 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 60 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 61 | ) 62 | install(TARGETS server 63 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 64 | ) 65 | -------------------------------------------------------------------------------- /nav_grid_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nav_grid_server 4 | 0.3.1 5 | Customizable tools for publishing images as NavGrids or OccupancyGrids 6 | 7 | David V. Lu!! 8 | 9 | BSD 10 | 11 | catkin 12 | libopencv-dev 13 | nav_2d_utils 14 | nav_grid 15 | nav_grid_iterators 16 | nav_grid_pub_sub 17 | nav_msgs 18 | roscpp 19 | map_server 20 | roslib 21 | roslint 22 | 23 | 24 | -------------------------------------------------------------------------------- /nav_grid_server/test/data/alpha.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_grid_server/test/data/alpha.png -------------------------------------------------------------------------------- /nav_grid_server/test/data/map.yaml: -------------------------------------------------------------------------------- 1 | image: spectrum.png 2 | resolution: 0.050000 3 | origin: [0, 0, 0.000000] 4 | occupied_thresh: 0.650000 5 | free_thresh: 0.196000 6 | negate: 0 7 | -------------------------------------------------------------------------------- /nav_grid_server/test/data/spectrum.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_grid_server/test/data/spectrum.pgm -------------------------------------------------------------------------------- /nav_grid_server/test/data/spectrum.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/nav_grid_server/test/data/spectrum.png -------------------------------------------------------------------------------- /robot_nav_tools/color_util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(color_util) 3 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror") 4 | if(NOT "${CMAKE_CXX_STANDARD}") 5 | set(CMAKE_CXX_STANDARD 14) 6 | endif() 7 | 8 | find_package(catkin REQUIRED COMPONENTS std_msgs) 9 | 10 | catkin_package( 11 | CATKIN_DEPENDS std_msgs 12 | INCLUDE_DIRS include 13 | LIBRARIES color_util 14 | ) 15 | include_directories(include ${catkin_INCLUDE_DIRS}) 16 | 17 | add_library(color_util src/convert.cpp) 18 | target_link_libraries(color_util ${catkin_LIBRARIES}) 19 | 20 | if(CATKIN_ENABLE_TESTING) 21 | find_package(catkin REQUIRED COMPONENTS roslint) 22 | roslint_cpp() 23 | roslint_add_test() 24 | 25 | catkin_add_gtest(utest test/utest.cpp) 26 | target_link_libraries(utest color_util ${catkin_LIBRARIES}) 27 | add_dependencies(utest ${catkin_EXPORTED_TARGETS} ${color_util_EXPORTED_TARGETS}) 28 | endif() 29 | 30 | install(TARGETS color_util 31 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 32 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 33 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 34 | ) 35 | install(DIRECTORY include/${PROJECT_NAME}/ 36 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 37 | ) 38 | -------------------------------------------------------------------------------- /robot_nav_tools/color_util/README.md: -------------------------------------------------------------------------------- 1 | # color_util 2 | An almost dependency-less library for converting between color spaces 3 | 4 | ## Datatypes 5 | This package contains representations for two different color spaces with two different datatypes in [include/color_util/types.h](include/color_util/types.h): 6 | * Red-Green-Blue-Alpha (`RGBA`) 7 | * Hue-Saturation-Value-Alpha (`HSVA`). 8 | 9 | Each of these are represented with two datatypes. 10 | * Four `double`s with values ranging `[0.0, 1.0]` 11 | * Four `unsigned char`s with values ranging `[0, 255]`. 12 | 13 | Note that with `unsigned char`, the color is represented by a total of 24 bits, which we use for notation. The four resulting types are 14 | * `ColorRGBA` - `RGBA/double` 15 | * `ColorRGBA24` - `RGBA/unsigned char` 16 | * `ColorHSVA` - `HSVA/double` 17 | * `ColorHSVA24` - `HSVA/unsigned char` 18 | 19 | ## Conversions 20 | With [include/color_util/convert.h](include/color_util/convert.h), you can convert between the above types and to `std_msgs::ColorRGBA` (which has floating point as its datatype). 21 | 22 | ## Blending 23 | With [include/color_util/blend.h](include/color_util/blend.h), you can create mixtures of two different colors. There are three options. 24 | 25 | | Blend Method | Image | Note | 26 | | ------------ | ----- | ---- | 27 | | `rgbaBlend` | ![rgba blend example](doc/blend00.png) | Linear interpolation of the `RGBA` values. Note that the bars in the middle are less saturated than the edges. | 28 | | `hueBlend` | ![hue blend example](doc/blend01.png) | Linear interpolation of the `HSVA` values. In this example, it goes from red (hue=0.0) to blue (hue=0.6) through green (hue=0.3) | 29 | | `hueBlendPlus` | ![hue blend plus example](doc/blend02.png) | Respects the circular nature of the hue representation and uses the shortest linear interpolation of the `HSVA` values. In this example, it goes from red (hue=0.0) to blue (hue=0.6) through magenta (hue=0.8) | 30 | 31 | You can experiment with the blending methods by running `roslaunch robot_nav_viz_demos spectrum_demo.launch`. 32 | 33 | ## Named Colors 34 | For certain applications, there is a need for accessing specific colors, and it can be annoying to have to specify hex values for each individual color. Other times, you may want to access a list of some number of unique colors. For this, this package provides the [`named_colors` header](include/color_util/named_colors.h) which allows you to access an array of 55 named colors either through a vector or an enum. The colors are made up of 18 colors, each with a standard, light and dark variant, plus transparent. 35 | 36 | ![color values](doc/named.png) 37 | 38 | The list can be accessed with `color_util::getNamedColors()` and individual colors can be grabbed with `color_util::get(NamedColor::RED)` 39 | -------------------------------------------------------------------------------- /robot_nav_tools/color_util/doc/blend00.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/robot_nav_tools/color_util/doc/blend00.png -------------------------------------------------------------------------------- /robot_nav_tools/color_util/doc/blend01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/robot_nav_tools/color_util/doc/blend01.png -------------------------------------------------------------------------------- /robot_nav_tools/color_util/doc/blend02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/robot_nav_tools/color_util/doc/blend02.png -------------------------------------------------------------------------------- /robot_nav_tools/color_util/doc/named.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/robot_nav_tools/color_util/doc/named.png -------------------------------------------------------------------------------- /robot_nav_tools/color_util/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | color_util 4 | 0.3.1 5 | An almost dependency-less library for converting between color spaces 6 | David V. Lu!! 7 | BSD 8 | 9 | catkin 10 | std_msgs 11 | roslint 12 | 13 | -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_rviz_plugins/icons/classes/NavGridOfChars.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/robot_nav_tools/robot_nav_rviz_plugins/icons/classes/NavGridOfChars.png -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_rviz_plugins/icons/classes/NavGridOfDoubles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/robot_nav_tools/robot_nav_rviz_plugins/icons/classes/NavGridOfDoubles.png -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_rviz_plugins/icons/classes/OccupancyGrid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/robot_nav_tools/robot_nav_rviz_plugins/icons/classes/OccupancyGrid.png -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_rviz_plugins/icons/classes/Path2D.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/robot_nav_tools/robot_nav_rviz_plugins/icons/classes/Path2D.png -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_rviz_plugins/icons/classes/Polygon2D.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/robot_nav_tools/robot_nav_rviz_plugins/icons/classes/Polygon2D.png -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_rviz_plugins/icons/classes/Polygon3D.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/robot_nav_tools/robot_nav_rviz_plugins/icons/classes/Polygon3D.png -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_rviz_plugins/icons/classes/Polygons.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/robot_nav_tools/robot_nav_rviz_plugins/icons/classes/Polygons.png -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_rviz_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_nav_rviz_plugins 4 | 0.3.1 5 | RViz visualizations for robot_navigation datatypes 6 | David V. Lu!! 7 | BSD 8 | 9 | catkin 10 | color_util 11 | geometry_msgs 12 | nav_2d_msgs 13 | nav_2d_utils 14 | nav_core2 15 | nav_grid 16 | nav_grid_iterators 17 | nav_grid_pub_sub 18 | nav_msgs 19 | pluginlib 20 | qtbase5-dev 21 | roscpp 22 | rviz 23 | std_msgs 24 | roslint 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_rviz_plugins/robot_nav_rviz_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Palette with grayscale for [0, 100], blueish gray for unknown and other colors for "illegal" values 5 | 6 | 7 | 8 | 9 | Palette with invisible 0, blue/red spectrum for [1, 98], cyan for 99, purple for 100, blueish gray for unknown, 10 | and other colors for "illegal" values. 11 | 12 | 13 | 14 | 15 | Palette that covers a black to white spectrum. 16 | 17 | 18 | 19 | 20 | Rainbow hued palette from purple to red with transparent 0 21 | 22 | 23 | 24 | 25 | Rainbow hued palette from red to purple with transparent 0 26 | 27 | 28 | 29 | 30 | Palette with a gradient of blues from dark to light 31 | 32 | 33 | 34 | 35 | Palette with 55 unique colors for when adjacent values have no relation to each other. 0 is transparent. 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_rviz_plugins/rviz_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Display for nav_2d_msgs::NavGridOfChars / nav_2d_msgs::NavGridOfCharsUpdate 5 | nav_2d_msgs/NavGridOfChars 6 | 7 | 8 | Display for nav_2d_msgs::NavGridOfDoubles / nav_2d_msgs::NavGridOfDoublesUpdate 9 | nav_2d_msgs/NavGridOfDoubles 10 | 11 | 12 | Fancier version of MapDisplay 13 | nav_msgs/OccupancyGrid 14 | 15 | 16 | Display for nav_2d_msgs::Path2D 17 | nav_2d_msgs/Path2D 18 | 19 | 20 | nav_2d_msgs/Polygon2DStamped 21 | Display for nav_2d_msgs::Polygon2D 22 | 23 | 24 | geometry_msgs/PolygonStamped 25 | Display for geometry_msgs::PolygonStamped 26 | 27 | 28 | nav_2d_msgs/Polygon2DCollection 29 | Display for nav_2d_msgs::Polygon2DCollection 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(robot_nav_tools) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_nav_tools 4 | 0.3.1 5 | A collection of tools / accessories for the robot_navigation packages 6 | David V. Lu!! 7 | BSD 8 | 9 | catkin 10 | color_util 11 | robot_nav_rviz_plugins 12 | robot_nav_viz_demos 13 | rqt_dwb_plugin 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_viz_demos/README.md: -------------------------------------------------------------------------------- 1 | # robot_nav_viz_demos 2 | 3 | Demos for testing/demonstrating `robot_nav_rviz_plugins` and `color_util` packages 4 | 5 | ## Color Util Demos 6 | To see a simple tool for visualizing `color_util` blending, run 7 | `roslaunch robot_nav_viz_demos spectrum_demo.launch`. To see what all the named colors look like, run `roslaunch robot_nav_viz_demos named_colors_demo.launch` 8 | 9 | ## Nav Grid Demo 10 | `roslaunch robot_nav_viz_demos grid_demo.launch` 11 | 12 | This will launch two processes. 13 | * `pong` is a simple script for generating `NavGridOfDoubles` and `NavGridOfDoublesUpdate` data with an ever-expanding range of values. 14 | * Note that `0` is set as the Ignore Value, which means that it is set to transparent (with some palettes) and the min/max range ignores that value. 15 | * The second plays back the data from `demo_grids.bag` which contains `NavGridOfChars` and `NavGridOfCharsUpdate` data. 16 | * The bag data only contains 6 values (by design) and thus requires custom palettes to fully appreciate. There are two contained within `demo_palettes.cpp`, called `mega` and `green`. This demonstrates the capabilities of the `pluginlib` approach for the palettes. 17 | 18 | ## Polygons Demo 19 | `roslaunch robot_nav_viz_demos polygons.launch` 20 | 21 | This shows off the new Polygon-based display types. 22 | * In the center, is a single Polygon2DStamped in the form of a simple star. Like all of the Polygon displays here, you can choose to display the outline, the filled in polygon, or both, as well as customize colors for each. 23 | * Outside of that is a Polygon2DCollection, i.e. an array of possibly Complex polygons (i.e. polygons with holes). Each of the rings represents a polygon with an outer and inner outline. There are three different fill-color styles you can choose. By default, it shows everything in one color. You can also switch it to use color information from the message, which should display a rainbow. You can also choose a set of unique colors from the `color_util` package if the color information is not included in the message. 24 | * The small star spinning farthest away has the type `geometry_msgs::PolygonStamped`. It is the same as the standard `rviz::PolygonDisplay` except it can also be filled in. You can enable the old-style polygon display for comparison. 25 | -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_viz_demos/data/demo_grids.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/robot_nav_tools/robot_nav_viz_demos/data/demo_grids.bag -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_viz_demos/data/demo_grids.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Name: Displays 4 | - Class: rviz/Views 5 | Name: Views 6 | Visualization Manager: 7 | Displays: 8 | - Alpha: 1 9 | Class: NavGridOfChars 10 | Color Scheme: mega 11 | Draw Behind: false 12 | Enabled: true 13 | Name: NavGridOfChars 14 | Topic: /grid 15 | Value: true 16 | - Alpha: 0.7 17 | Class: NavGridOfDoubles 18 | Color Scheme: rainbow2 19 | Draw Behind: true 20 | Enabled: true 21 | Ignore Value: 0 22 | Name: NavGridOfDoubles 23 | Topic: /pong/grid 24 | Value: true 25 | Global Options: 26 | Fixed Frame: /map 27 | Tools: 28 | - Class: rviz/MoveCamera 29 | Views: 30 | Current: 31 | Angle: 0 32 | Class: rviz/TopDownOrtho 33 | Scale: 25 34 | Value: TopDownOrtho (rviz) 35 | X: 9.5 36 | Y: 4.2 37 | Window Geometry: 38 | Height: 846 39 | Width: 1200 40 | -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_viz_demos/data/demo_polygons.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Name: Displays 4 | Tree Height: 549 5 | - Class: rviz/Views 6 | Name: Views 7 | Visualization Manager: 8 | Displays: 9 | - Class: rviz/Grid 10 | Name: Grid 11 | Value: true 12 | - Class: Polygon2D 13 | Enabled: true 14 | Name: Polygon2D 15 | Topic: /polygon/polygon 16 | - Class: Polygons 17 | Enabled: true 18 | Name: Polygons 19 | Topic: /polygon/polygons 20 | - Class: rviz/Polygon 21 | Enabled: false 22 | Name: Old Style Polygon3D 23 | Topic: /polygon/polygon3d 24 | - Alpha: 1 25 | Class: Polygon3D 26 | Enabled: true 27 | Fill Color: 255; 255; 255 28 | Name: New Style Polygon3D 29 | Outline Color: 239; 41; 41 30 | Topic: /polygon/polygon3d 31 | Tools: 32 | - Class: rviz/MoveCamera 33 | Views: 34 | Current: 35 | Distance: 14 36 | Pitch: 0.870 37 | Value: Orbit (rviz) 38 | Yaw: 1.0 39 | Window Geometry: 40 | Height: 846 41 | Width: 1200 42 | X: 60 43 | Y: 0 44 | -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_viz_demos/data/named_colors_demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Name: Displays 4 | Tree Height: 251 5 | - Class: rviz/Views 6 | Expanded: 7 | - /Current View1 8 | Name: Views 9 | Splitter Ratio: 0.5 10 | Visualization Manager: 11 | Displays: 12 | - Alpha: 1 13 | Class: NavGridOfChars 14 | Color Scheme: distinct 15 | Enabled: true 16 | Name: NavGridOfChars 17 | Topic: /named_colors_demo/grid 18 | Value: true 19 | Tools: 20 | - Class: rviz/MoveCamera 21 | Views: 22 | Current: 23 | Angle: 0 24 | Class: rviz/TopDownOrtho 25 | Name: Current View 26 | Near Clip Distance: 0.009999999776482582 27 | Scale: 85 28 | Value: TopDownOrtho (rviz) 29 | X: 4.5 30 | Y: 1 31 | Window Geometry: 32 | Height: 642 33 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000228fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000017e000000c900fffffffb0000000a0056006900650077007301000001c1000000a4000000a400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000228fc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003540000022800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 34 | Width: 1200 35 | X: 108 36 | Y: 368 37 | -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_viz_demos/data/spectrum.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Name: Displays 4 | Property Tree Widget: 5 | Expanded: 6 | - /Global Options1 7 | - /SpectrumDisplay1 8 | Tree Height: 549 9 | - Class: rviz/Views 10 | Expanded: 11 | - /Current View1 12 | Name: Views 13 | Visualization Manager: 14 | Displays: 15 | - Alpha A: 1 16 | Alpha B: 1 17 | Class: robot_nav_rviz_plugins::SpectrumDisplay 18 | Color A: 239; 41; 41 19 | Color B: 41; 0; 226 20 | Enabled: true 21 | Name: SpectrumDisplay 22 | Value: true 23 | Tools: 24 | - Class: rviz/MoveCamera 25 | Views: 26 | Current: 27 | Angle: 1.57 28 | Class: rviz/TopDownOrtho 29 | Scale: 460 30 | Value: TopDownOrtho (rviz) 31 | X: 0.5 32 | Y: 0.5 33 | Saved: null 34 | -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_viz_demos/launch/grid_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_viz_demos/launch/named_colors_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_viz_demos/launch/polygons.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_viz_demos/launch/spectrum_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_viz_demos/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_nav_viz_demos 4 | 0.3.1 5 | Demos for testing/demonstrating the robot_nav_rviz_plugins and color_util packages 6 | David V. Lu!! 7 | BSD 8 | 9 | catkin 10 | angles 11 | color_util 12 | geometry_msgs 13 | nav_2d_msgs 14 | nav_2d_utils 15 | nav_grid 16 | nav_grid_iterators 17 | nav_grid_pub_sub 18 | pluginlib 19 | robot_nav_rviz_plugins 20 | rosbag 21 | roscpp 22 | rviz 23 | roslaunch 24 | roslint 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_viz_demos/robot_nav_rviz_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /robot_nav_tools/robot_nav_viz_demos/rviz_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Simple tool for visualizing color_util blending 4 | 5 | 6 | -------------------------------------------------------------------------------- /robot_nav_tools/rqt_dwb_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(rqt_dwb_plugin) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_python_setup() 6 | 7 | catkin_package() 8 | 9 | if(CATKIN_ENABLE_TESTING) 10 | find_package(roslint) 11 | roslint_python() 12 | roslint_add_test() 13 | endif() 14 | 15 | install(FILES plugins.xml 16 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 17 | ) 18 | install(FILES scripts/live_panel 19 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts 20 | ) 21 | -------------------------------------------------------------------------------- /robot_nav_tools/rqt_dwb_plugin/README.md: -------------------------------------------------------------------------------- 1 | # rqt_dwb_plugin 2 | 3 | This package allows you to view `dwb_msgs/LocalPlanEvaluation` information visually using `rqt`. Currently only `x/theta` velocities are supported. The information can be viewed inside of `rqt_bag` or as an independent application. For a high level view of how `rqt_bag` plugins work, 4 | check out [this tutorial](http://wiki.ros.org/rqt_bag/Tutorials/Create%20an%20rqt_bag%20plugin). 5 | 6 | ## Eval View 7 | You can show detailed information about a single `LocalPlanEvaluation` in one of two ways: 8 | * Within `rqt_bag` you can right click the `LocalPlanEvaluation` data much the same way you 9 | would view the raw values of the message. 10 | * You can also view live data with an independent application by running 11 | `rosrun rqt_dwb_plugin live_panel _ns:=/move_base/DWBLocalPlanner` 12 | 13 | ![widget screenshot](doc/widget0.png) 14 | 15 | This view has four different components. 16 | 17 | * **Trajectory Cloud** - 18 | The top panel shows the Trajectory "Cloud", with each of the poses for each of the trajectories drawn relative to the robot's starting coordinates. 19 | 20 | * **Velocity Space** - 21 | The left panel shows each of the individual trajectories being evaluated in velocity space. The x axis represents the x component and the y axis represents the theta component. The highest and lowest values are displayed in the axis labels. 22 | 23 | * **Sorted Scores** - 24 | The right panel shows a table of the total score for each trajectory, as well as the x/theta velocity for that trajectory. They are sorted with increasing scores, thus the best option will always be first. 25 | 26 | * **Detailed Scores** - 27 | The bottom panel shows the detailed critic scores for the selected trajectories. 28 | 29 | ### Selecting Additional Trajectories 30 | The left and right panels give you the ability to select additional trajectories to be highlighted. Clicking in either the velocity space or sorted scores will add a trajectory from each panel in a new color. You can click again to remove it. 31 | 32 | ![widget screenshot](doc/widget1.png) 33 | 34 | ### Additional Topics 35 | Note that if `/transformed_global_plan` and `/velocity` also exist in the bag file with the same namespace, the plan will be drawn into the trajectory cloud, and the current velocity will be drawn into the velocity space. 36 | 37 | ## Timeline Plot 38 | This feature only works within `rqt_bag`. If you turn on "Thumbnails" ![thumbnail logo](doc/thumbnail.png) for `LocalPlanEvaluation` data, it will show a plot of the best velocity values. 39 | 40 | ![screenshot of timeline](doc/timeline.png) 41 | 42 | * The black line is the x component and blue is the theta component. 43 | * If local planning failed, the values will be 0 and displayed in red, as seen at the end of the above bag. 44 | -------------------------------------------------------------------------------- /robot_nav_tools/rqt_dwb_plugin/doc/thumbnail.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/robot_nav_tools/rqt_dwb_plugin/doc/thumbnail.png -------------------------------------------------------------------------------- /robot_nav_tools/rqt_dwb_plugin/doc/timeline.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/robot_nav_tools/rqt_dwb_plugin/doc/timeline.png -------------------------------------------------------------------------------- /robot_nav_tools/rqt_dwb_plugin/doc/widget0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/robot_nav_tools/rqt_dwb_plugin/doc/widget0.png -------------------------------------------------------------------------------- /robot_nav_tools/rqt_dwb_plugin/doc/widget1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/robot_nav_tools/rqt_dwb_plugin/doc/widget1.png -------------------------------------------------------------------------------- /robot_nav_tools/rqt_dwb_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rqt_dwb_plugin 4 | 0.3.1 5 | View dwb_msgs/LocalPlanEvaluation information visually using rqt 6 | David V. Lu!! 7 | BSD 8 | 9 | catkin 10 | python3-setuptools 11 | 12 | dwb_msgs 13 | geometry_msgs 14 | nav_2d_utils 15 | python_qt_binding 16 | rospy 17 | rqt_bag 18 | roslint 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /robot_nav_tools/rqt_dwb_plugin/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Plugin for rqt_bag to examine dwb_msgs/LocalPlanEvaluation messages 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /robot_nav_tools/rqt_dwb_plugin/scripts/live_panel: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2018-2019, Locus Robotics 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of the copyright holder nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | import sys 34 | 35 | from PyQt5.QtWidgets import QApplication 36 | 37 | from dwb_msgs.msg import LocalPlanEvaluation 38 | 39 | import rospy 40 | 41 | from rqt_dwb_plugin.dwb_widget import DWBWidget 42 | 43 | 44 | if __name__ == '__main__': 45 | rospy.init_node('dwb_live_panel') 46 | 47 | app = QApplication(rospy.myargv()) 48 | 49 | dwb_widget = DWBWidget(None) 50 | dwb_widget.setGeometry(0, 0, 700, 1000) 51 | dwb_widget.show() 52 | 53 | ns = rospy.get_param('~ns') 54 | sub = rospy.Subscriber('{}/evaluation'.format(ns), 55 | LocalPlanEvaluation, dwb_widget.setEvaluation) 56 | 57 | sys.exit(app.exec_()) 58 | -------------------------------------------------------------------------------- /robot_nav_tools/rqt_dwb_plugin/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | package_info = generate_distutils_setup( 7 | packages=['rqt_dwb_plugin'], 8 | package_dir={'': 'src'} 9 | ) 10 | 11 | setup(**package_info) 12 | -------------------------------------------------------------------------------- /robot_nav_tools/rqt_dwb_plugin/src/rqt_dwb_plugin/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/robot_navigation/fabbe8e070b368d6653df8a00fedf1aa7dfeb147/robot_nav_tools/rqt_dwb_plugin/src/rqt_dwb_plugin/__init__.py -------------------------------------------------------------------------------- /robot_nav_tools/rqt_dwb_plugin/src/rqt_dwb_plugin/bounds.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2018-2019, Locus Robotics 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of the copyright holder nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | from .util import subtractPose 34 | 35 | 36 | class Bounds(object): 37 | """Tracks the extreme bounds of all the relative poses within a LocalPlanEvaluation.""" 38 | 39 | def __init__(self, initial_evaluation=None): 40 | self.x_min = self.y_min = 0.0 41 | self.x_max = self.y_max = 0.0 42 | if initial_evaluation: 43 | self.process_evaluation(initial_evaluation) 44 | 45 | def process_evaluation(self, msg): 46 | for twist in msg.twists: 47 | self.check_pose(subtractPose(twist.traj.poses[-1], twist.traj.poses[0])) 48 | 49 | def check_pose(self, pose): 50 | self.x_min = min(self.x_min, pose.x) 51 | self.x_max = max(self.x_max, pose.x) 52 | self.y_min = min(self.y_min, pose.y) 53 | self.y_max = max(self.y_max, pose.y) 54 | -------------------------------------------------------------------------------- /robot_nav_tools/rqt_dwb_plugin/src/rqt_dwb_plugin/dwb_plugin.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2018-2019, Locus Robotics 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of the copyright holder nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | from rqt_bag.plugins.plugin import Plugin 34 | 35 | from .eval_message_view import EvalView 36 | from .timeline_plotter import TimelinePlotter 37 | 38 | 39 | class DWBPlugin(Plugin): 40 | """Delegates handling messages to two other classes.""" 41 | 42 | def __init__(self): 43 | pass 44 | 45 | def get_view_class(self): 46 | return EvalView 47 | 48 | def get_renderer_class(self): 49 | return TimelinePlotter 50 | 51 | def get_message_types(self): 52 | return ['dwb_msgs/LocalPlanEvaluation'] 53 | -------------------------------------------------------------------------------- /robot_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(robot_navigation) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /robot_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_navigation 4 | 0.3.1 5 | The robot_navigation package 6 | 7 | David V. Lu!! 8 | 9 | BSD 10 | 11 | catkin 12 | costmap_queue 13 | dlux_global_planner 14 | dlux_plugins 15 | dwb_critics 16 | dwb_local_planner 17 | dwb_msgs 18 | dwb_plugins 19 | global_planner_tests 20 | locomotor 21 | locomotor_msgs 22 | locomove_base 23 | nav_2d_msgs 24 | nav_2d_utils 25 | nav_core2 26 | nav_core_adapter 27 | nav_grid 28 | nav_grid_iterators 29 | nav_grid_pub_sub 30 | nav_grid_server 31 | 32 | 33 | 34 | 35 | --------------------------------------------------------------------------------