├── .clang-format ├── .github └── workflows │ ├── ci_focal.yml │ └── clang_format.yml ├── .gitignore ├── LICENSE ├── README.md ├── assets ├── empty_map.gif ├── empty_map0.png ├── map_with_many_homotopy_classes.gif ├── map_with_many_homotopy_classes0.png ├── map_with_single_cube.gif ├── map_with_single_cube0.png ├── map_with_single_narrow_passage_gap.gif ├── map_with_single_narrow_passage_gap0.png └── rviz_panel.png ├── ompl_2d_rviz_visualizer ├── CMakeLists.txt └── package.xml ├── ompl_2d_rviz_visualizer_msgs ├── CMakeLists.txt ├── package.xml └── srv │ ├── MapBounds.srv │ ├── Plan.srv │ ├── Reset.srv │ └── State.srv └── ompl_2d_rviz_visualizer_ros ├── CMakeLists.txt ├── config ├── planner_params.yaml └── rviz │ └── visualizer.rviz ├── include └── ompl_2d_rviz_visualizer_ros │ ├── collision_checker.h │ ├── map_loader.h │ ├── map_utils.h │ └── rviz_renderer.h ├── launch └── visualizer.launch ├── map ├── empty_map │ ├── empty_map.pgm │ └── empty_map.yaml ├── map_with_many_homotopy_classes │ ├── map_with_many_homotopy_classes.pgm │ └── map_with_many_homotopy_classes.yaml ├── map_with_single_cube │ ├── map_with_single_cube.pgm │ └── map_with_single_cube.yaml └── map_with_single_narrow_passage_gap │ ├── map_with_single_narrow_passage_gap.pgm │ └── map_with_single_narrow_passage_gap.yaml ├── nodelet_plugins.xml ├── package.xml ├── plugin_description.xml └── src ├── map_loader.cpp ├── rviz_control_panel.cpp ├── rviz_control_panel.h ├── rviz_renderer.cpp └── visualizer_nodelet.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -2 4 | ConstructorInitializerIndentWidth: 2 5 | AlignEscapedNewlinesLeft: false 6 | AlignTrailingComments: true 7 | AllowAllParametersOfDeclarationOnNextLine: false 8 | AllowShortIfStatementsOnASingleLine: false 9 | AllowShortLoopsOnASingleLine: false 10 | AllowShortFunctionsOnASingleLine: None 11 | AlwaysBreakTemplateDeclarations: true 12 | AlwaysBreakBeforeMultilineStrings: true 13 | BreakBeforeBinaryOperators: false 14 | BreakBeforeTernaryOperators: false 15 | BreakConstructorInitializersBeforeComma: true 16 | BinPackParameters: true 17 | ColumnLimit: 120 18 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 19 | DerivePointerBinding: false 20 | PointerBindsToType: true 21 | ExperimentalAutoDetectBinPacking: false 22 | IndentCaseLabels: true 23 | MaxEmptyLinesToKeep: 1 24 | NamespaceIndentation: None 25 | ObjCSpaceBeforeProtocolList: true 26 | PenaltyBreakBeforeFirstCallParameter: 19 27 | PenaltyBreakComment: 60 28 | PenaltyBreakString: 1 29 | PenaltyBreakFirstLessLess: 1000 30 | PenaltyExcessCharacter: 1000 31 | PenaltyReturnTypeOnItsOwnLine: 90 32 | SpacesBeforeTrailingComments: 2 33 | Cpp11BracedListStyle: false 34 | Standard: Auto 35 | IndentWidth: 2 36 | TabWidth: 2 37 | UseTab: Never 38 | IndentFunctionDeclarationAfterType: false 39 | SpacesInParentheses: false 40 | SpacesInAngles: false 41 | SpaceInEmptyParentheses: false 42 | SpacesInCStyleCastParentheses: false 43 | SpaceAfterControlStatementKeyword: true 44 | SpaceBeforeAssignmentOperators: true 45 | ContinuationIndentWidth: 4 46 | SortIncludes: false 47 | SpaceAfterCStyleCast: false 48 | 49 | # Configure each individual brace in BraceWrapping 50 | BreakBeforeBraces: Custom 51 | 52 | # Control of individual brace wrapping cases 53 | BraceWrapping: { 54 | AfterClass: 'true' 55 | AfterControlStatement: 'true' 56 | AfterEnum : 'true' 57 | AfterFunction : 'true' 58 | AfterNamespace : 'true' 59 | AfterStruct : 'true' 60 | AfterUnion : 'true' 61 | BeforeCatch : 'true' 62 | BeforeElse : 'true' 63 | IndentBraces : 'false' 64 | } 65 | ... 66 | -------------------------------------------------------------------------------- /.github/workflows/ci_focal.yml: -------------------------------------------------------------------------------- 1 | name: CI - Ubuntu Focal 2 | 3 | on: 4 | # direct pushes to protected branches are not supported 5 | pull_request: 6 | schedule: 7 | # 7am UTC, 12am PDT 8 | - cron: '0 7 * * *' 9 | # allow manually starting this workflow 10 | workflow_dispatch: 11 | 12 | jobs: 13 | industrial_ci: 14 | name: ROS Focal ${{ matrix.ROS_DISTRO }} (${{ matrix.ROS_REPO }}) 15 | runs-on: ubuntu-20.04 16 | strategy: 17 | matrix: # matrix is the product of entries 18 | ROS_DISTRO: [noetic] 19 | ROS_REPO: [main, testing] 20 | env: 21 | CCACHE_DIR: "${{ github.workspace }}/.ccache" # directory for ccache (and how we enable ccache in industrial_ci) 22 | steps: 23 | - uses: actions/checkout@v2 # clone target repository 24 | - uses: actions/cache@v2 # fetch/store the directory used by ccache before/after the ci run 25 | with: 26 | path: ${{ env.CCACHE_DIR }} 27 | key: ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}-${{github.run_id}} 28 | restore-keys: | 29 | ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}- 30 | - uses: 'ros-industrial/industrial_ci@master' # run industrial_ci 31 | env: 32 | ROS_DISTRO: ${{ matrix.ROS_DISTRO }} 33 | ROS_REPO: ${{ matrix.ROS_REPO }} 34 | -------------------------------------------------------------------------------- /.github/workflows/clang_format.yml: -------------------------------------------------------------------------------- 1 | name: Clang-Format 2 | 3 | on: 4 | # direct pushes to protected branches are not supported 5 | pull_request: 6 | schedule: 7 | # 7am UTC, 12am PDT 8 | - cron: '0 7 * * *' 9 | # allow manually starting this workflow 10 | workflow_dispatch: 11 | 12 | jobs: 13 | industrial_ci: 14 | name: ROS Focal ${{ matrix.ROS_DISTRO }} (${{ matrix.ROS_REPO }}) 15 | runs-on: ubuntu-20.04 16 | strategy: 17 | matrix: # matrix is the product of entries 18 | ROS_DISTRO: [noetic] 19 | ROS_REPO: [main] 20 | env: 21 | CCACHE_DIR: "${{ github.workspace }}/.ccache" # directory for ccache (and how we enable ccache in industrial_ci) 22 | CLANG_FORMAT_CHECK: file 23 | CLANG_FORMAT_VERSION: 8 24 | 25 | steps: 26 | - uses: actions/checkout@v2 # clone target repository 27 | - uses: actions/cache@v2 # fetch/store the directory used by ccache before/after the ci run 28 | with: 29 | path: ${{ env.CCACHE_DIR }} 30 | key: ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}-${{github.run_id}} 31 | restore-keys: | 32 | ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}- 33 | - uses: 'ros-industrial/industrial_ci@master' # run industrial_ci 34 | env: 35 | ROS_DISTRO: ${{ matrix.ROS_DISTRO }} 36 | ROS_REPO: ${{ matrix.ROS_REPO }} 37 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | .vscode -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2021, Phone Thiha Kyaw 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 are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | OMPL 2D Rviz Visualizer 2 | ===== 3 | [![License](https://img.shields.io/badge/License-BSD%202--Clause-blue.svg)](https://opensource.org/licenses/BSD-2-Clause) 4 | ![Clang Format](https://github.com/mlsdpk/ompl_2d_rviz_visualizer/actions/workflows/clang_format.yml/badge.svg) 5 | ![CI Focal](https://github.com/mlsdpk/ompl_2d_rviz_visualizer/actions/workflows/ci_focal.yml/badge.svg) 6 | 7 | Visualizing, animating and debugging Open Motion Planning Library (OMPL) algorithms in ROS Rviz. The package allows OMPL planners to find paths in R^2 state space with path minimizing objectives and utilizes the occupancy grid maps for collision checking. We have also added a rviz panel plugin to interface with OMPL. 8 | 9 |

10 | 11 | 12 | 13 | 14 |

15 | 16 | ## Installation 17 | 18 | ### Build from source 19 | 20 | Clone this package into your src folder of catkin_workspace: 21 | ``` 22 | cd /src 23 | git clone https://github.com/mlsdpk/ompl_2d_rviz_visualizer.git 24 | ``` 25 | 26 | Install necessary dependencies by running the following command in the root of your workspace: 27 | ``` 28 | cd .. 29 | rosdep install -y --from-paths src --ignore-src --rosdistro 30 | ``` 31 | 32 | Then compile with catkin: 33 | ``` 34 | catkin_make 35 | ``` 36 | 37 | ## Rviz Control Panel for interfacing with OMPL 38 | 39 | We provide an easy-to-use rviz panel plugin for interfacing with OMPL. 40 | 41 | ![](assets/rviz_panel.png) 42 | 43 | ## Maps 44 | 45 | Several occupancy grid maps are specifically chosen for testing OMPL planners, which can be found in most sampling-based planning literatures. In order to select a map, user has to manually provide the name of the map to the `map_name` argument in the [launch file](https://github.com/mlsdpk/ompl_2d_rviz_visualizer/blob/master/ompl_2d_rviz_visualizer_ros/launch/visualizer.launch). Current version of the visualizer provides four different maps: 46 | 1. empty_map 47 | 2. map_with_single_cube 48 | 3. map_with_many_homotopy_classes 49 | 4. map_with_single_narrow_passage_gap 50 | 51 | 1 | 2 | 3 | 4 52 | :-:|:-:|:-:|:-: 53 | ||| 54 | 55 | ## Usage 56 | 57 | Run the following launch file to use the visualizer. You can directly pass map names via command line or change it manually in the file. 58 | 59 | ``` 60 | source /devel/setup.bash 61 | roslaunch ompl_2d_rviz_visualizer_ros visualizer.launch map_name:=empty_map 62 | ``` 63 | -------------------------------------------------------------------------------- /assets/empty_map.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/ompl_2d_rviz_visualizer/39ea31dbab97eec023d29b27f24bab2286c3370a/assets/empty_map.gif -------------------------------------------------------------------------------- /assets/empty_map0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/ompl_2d_rviz_visualizer/39ea31dbab97eec023d29b27f24bab2286c3370a/assets/empty_map0.png -------------------------------------------------------------------------------- /assets/map_with_many_homotopy_classes.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/ompl_2d_rviz_visualizer/39ea31dbab97eec023d29b27f24bab2286c3370a/assets/map_with_many_homotopy_classes.gif -------------------------------------------------------------------------------- /assets/map_with_many_homotopy_classes0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/ompl_2d_rviz_visualizer/39ea31dbab97eec023d29b27f24bab2286c3370a/assets/map_with_many_homotopy_classes0.png -------------------------------------------------------------------------------- /assets/map_with_single_cube.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/ompl_2d_rviz_visualizer/39ea31dbab97eec023d29b27f24bab2286c3370a/assets/map_with_single_cube.gif -------------------------------------------------------------------------------- /assets/map_with_single_cube0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/ompl_2d_rviz_visualizer/39ea31dbab97eec023d29b27f24bab2286c3370a/assets/map_with_single_cube0.png -------------------------------------------------------------------------------- /assets/map_with_single_narrow_passage_gap.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/ompl_2d_rviz_visualizer/39ea31dbab97eec023d29b27f24bab2286c3370a/assets/map_with_single_narrow_passage_gap.gif -------------------------------------------------------------------------------- /assets/map_with_single_narrow_passage_gap0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/ompl_2d_rviz_visualizer/39ea31dbab97eec023d29b27f24bab2286c3370a/assets/map_with_single_narrow_passage_gap0.png -------------------------------------------------------------------------------- /assets/rviz_panel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/ompl_2d_rviz_visualizer/39ea31dbab97eec023d29b27f24bab2286c3370a/assets/rviz_panel.png -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ompl_2d_rviz_visualizer) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ompl_2d_rviz_visualizer 4 | 1.0.0 5 | The ompl_2d_rviz_visualizer package 6 | 7 | Phone Thiha Kyaw 8 | Phone Thiha Kyaw 9 | 10 | TODO 11 | 12 | catkin 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ompl_2d_rviz_visualizer_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | std_msgs 6 | message_generation 7 | ) 8 | 9 | add_service_files( 10 | FILES 11 | Plan.srv 12 | Reset.srv 13 | State.srv 14 | MapBounds.srv 15 | ) 16 | 17 | generate_messages( 18 | DEPENDENCIES 19 | std_msgs 20 | ) 21 | 22 | catkin_package( 23 | CATKIN_DEPENDS std_msgs message_runtime 24 | ) -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ompl_2d_rviz_visualizer_msgs 4 | 1.0.0 5 | Custom ROS messages for ompl 2d rviz visualizer 6 | 7 | Phone Thiha Kyaw 8 | 9 | 10 | 11 | 12 | TODO 13 | 14 | Phone Thiha Kyaw 15 | 16 | catkin 17 | std_msgs 18 | message_generation 19 | message_runtime 20 | message_runtime 21 | 22 | -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_msgs/srv/MapBounds.srv: -------------------------------------------------------------------------------- 1 | --- 2 | float64 min_x 3 | float64 min_y 4 | float64 max_x 5 | float64 max_y 6 | -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_msgs/srv/Plan.srv: -------------------------------------------------------------------------------- 1 | # visualization mode 2 | # 0 - duration 3 | # 1 - iterations 4 | # 2 - animation mode 5 | uint8 mode 6 | 7 | # render planner data every n iteration 8 | # only used in animation mode 9 | uint32 animate_every 10 | 11 | # speed to control the animation (0.0 to 1.0) 12 | # only used in animation mode 13 | float64 animation_speed 14 | 15 | # planner id 16 | uint8 planner_id 17 | 18 | # optimization objective 19 | uint8 objective_id 20 | 21 | # termination conditions 22 | # time and iteration 23 | # note that, time termination condition is only available 24 | # in no animation mode (mode 0) 25 | 26 | # timed termination condition 27 | float64 duration 28 | 29 | # iteration termination condition 30 | uint32 iterations 31 | --- 32 | bool success -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_msgs/srv/Reset.srv: -------------------------------------------------------------------------------- 1 | bool clear_graph 2 | --- 3 | bool success -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_msgs/srv/State.srv: -------------------------------------------------------------------------------- 1 | float64 x 2 | float64 y 3 | --- 4 | bool success -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(ompl_2d_rviz_visualizer_ros) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | 9 | set(${PROJECT_NAME}_CATKIN_DEPS 10 | eigen_conversions 11 | roscpp 12 | rviz_visual_tools 13 | rviz 14 | ompl_2d_rviz_visualizer_msgs 15 | nav_msgs 16 | tf2 17 | ) 18 | 19 | find_package(ompl REQUIRED) 20 | find_package(catkin REQUIRED COMPONENTS ${${PROJECT_NAME}_CATKIN_DEPS}) 21 | find_package(Bullet REQUIRED) 22 | find_package(SDL REQUIRED) 23 | find_package(SDL_image REQUIRED) 24 | find_package(Boost REQUIRED COMPONENTS filesystem) 25 | find_package(yaml-cpp 0.6 REQUIRED) 26 | 27 | # Qt Stuff 28 | if(rviz_QT_VERSION VERSION_LESS "5") 29 | find_package(Qt4 ${rviz_QT_VERSION} REQUIRED QtCore QtGui) 30 | include(${QT_USE_FILE}) 31 | macro(qt_wrap_ui) 32 | qt4_wrap_ui(${ARGN}) 33 | endmacro() 34 | macro(qt_wrap_cpp) 35 | qt4_wrap_cpp(${ARGN}) 36 | endmacro() 37 | else() 38 | find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) 39 | set(QT_LIBRARIES Qt5::Widgets) 40 | macro(qt_wrap_ui) 41 | qt5_wrap_ui(${ARGN}) 42 | endmacro() 43 | macro(qt_wrap_cpp) 44 | qt5_wrap_cpp(${ARGN}) 45 | endmacro() 46 | endif() 47 | 48 | set(CMAKE_INCLUDE_CURRENT_DIR ON) 49 | set(CMAKE_AUTOMOC ON) 50 | add_definitions(-DQT_NO_KEYWORDS) 51 | 52 | catkin_package( 53 | INCLUDE_DIRS include 54 | LIBRARIES 55 | ${PROJECT_NAME}_nodelet 56 | ${PROJECT_NAME}_panel 57 | CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS} 58 | ) 59 | 60 | include_directories( 61 | include 62 | ${BULLET_INCLUDE_DIRS} 63 | ${OMPL_INCLUDE_DIRS} 64 | ${catkin_INCLUDE_DIRS} 65 | ${Boost_INCLUDE_DIRS} 66 | ${Eigen_INCLUDE_DIRS} 67 | ${YAML_CPP_INCLUDE_DIRS} 68 | ${SDL_INCLUDE_DIR} 69 | ${SDL_IMAGE_INCLUDE_DIRS} 70 | ) 71 | 72 | ## specify which header files need to be run through "moc", 73 | ## Qt's meta-object compiler. 74 | qt_wrap_cpp(MOC_FILES 75 | src/rviz_control_panel.h 76 | ) 77 | 78 | # visualizer nodelet 79 | add_library(${PROJECT_NAME}_nodelet 80 | src/visualizer_nodelet.cpp 81 | src/rviz_renderer.cpp 82 | src/map_loader.cpp 83 | ) 84 | target_link_libraries(${PROJECT_NAME}_nodelet 85 | ${BULLET_LIBRARIES} 86 | ${catkin_LIBRARIES} 87 | ${OMPL_LIBRARIES} 88 | ${SDL_LIBRARY} 89 | ${SDL_IMAGE_LIBRARIES} 90 | ) 91 | 92 | # control panel 93 | add_library(${PROJECT_NAME}_panel 94 | src/rviz_control_panel.cpp 95 | ${MOC_FILES} 96 | ) 97 | target_link_libraries(${PROJECT_NAME}_panel 98 | ${rviz_DEFAULT_PLUGIN_LIBRARIES} 99 | ${QT_LIBRARIES} 100 | ${catkin_LIBRARIES} 101 | ) 102 | 103 | ############# 104 | ## Install ## 105 | ############# 106 | 107 | install(TARGETS 108 | ${PROJECT_NAME}_nodelet 109 | ${PROJECT_NAME}_panel 110 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 111 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 112 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 113 | ) 114 | 115 | install(DIRECTORY include/${PROJECT_NAME}/ 116 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 117 | ) 118 | 119 | install(FILES 120 | nodelet_plugins.xml 121 | plugin_description.xml 122 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 123 | ) 124 | -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/config/planner_params.yaml: -------------------------------------------------------------------------------- 1 | ompl_planner_parameters: 2 | rrt_connect: 3 | intermediate_states: false 4 | range: 0.0 5 | 6 | rrt_star: 7 | delay_collision_checking: true 8 | focus_search: false 9 | goal_bias: 0.05 10 | informed_sampling: false 11 | new_state_rejection: false 12 | number_sampling_attempts: 100 13 | ordered_sampling: false 14 | ordering_batch_size: 1 15 | prune_threshold: 0.05 16 | pruned_measure: false 17 | range: 0.0 18 | rewire_factor: 1.1 19 | sample_rejection: false 20 | tree_pruning: false 21 | use_admissible_heuristic: true 22 | use_k_nearest: true 23 | 24 | ompl_planner_parameters_range: 25 | rrt_connect: 26 | intermediate_states: "0,1" 27 | range: "0.:1.:10000." 28 | 29 | rrt_star: 30 | delay_collision_checking: "0,1" 31 | focus_search: "0,1" 32 | goal_bias: "0.:.05:1." 33 | informed_sampling: "0,1" 34 | new_state_rejection: "0,1" 35 | number_sampling_attempts: "10:10:100000" 36 | ordered_sampling: "0,1" 37 | ordering_batch_size: "1:100:1000000" 38 | prune_threshold: "0.:.01:1." 39 | pruned_measure: "0,1" 40 | range: "0.:1.:10000." 41 | rewire_factor: "1.0:0.01:2.0" 42 | sample_rejection: "0,1" 43 | tree_pruning: "0,1" 44 | use_admissible_heuristic: "0,1" 45 | use_k_nearest: "0,1" 46 | -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/config/rviz/visualizer.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /MarkerArray1/Namespaces1 8 | - /Map1/Position1 9 | - /Grid1/Offset1 10 | Splitter Ratio: 0.47660312056541443 11 | Tree Height: 140 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | - Class: ompl_2d_rviz_visualizer_ros/OMPL_ControlPanel 32 | Name: OMPL ControlPanel 33 | Preferences: 34 | PromptSaveOnExit: true 35 | Toolbars: 36 | toolButtonStyle: 2 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Class: rviz/MarkerArray 41 | Enabled: true 42 | Marker Topic: /ompl_2d_rviz_visualizer_nodelet/rviz_visual_markers 43 | Name: MarkerArray 44 | Namespaces: 45 | {} 46 | Queue Size: 10000 47 | Value: true 48 | - Alpha: 0.699999988079071 49 | Class: rviz/Map 50 | Color Scheme: map 51 | Draw Behind: false 52 | Enabled: true 53 | Name: Map 54 | Topic: /ompl_2d_rviz_visualizer_nodelet/map 55 | Unreliable: false 56 | Use Timestamp: false 57 | Value: true 58 | - Alpha: 0.5 59 | Cell Size: 1 60 | Class: rviz/Grid 61 | Color: 160; 160; 164 62 | Enabled: true 63 | Line Style: 64 | Line Width: 0.029999999329447746 65 | Value: Lines 66 | Name: Grid 67 | Normal Cell Count: 0 68 | Offset: 69 | X: 2.5 70 | Y: 2.5 71 | Z: 0 72 | Plane: XY 73 | Plane Cell Count: 5 74 | Reference Frame: 75 | Value: true 76 | - Alpha: 1 77 | Class: rviz/Axes 78 | Enabled: true 79 | Length: 0.5 80 | Name: Axes 81 | Radius: 0.019999999552965164 82 | Reference Frame: 83 | Value: true 84 | Enabled: true 85 | Global Options: 86 | Background Color: 136; 138; 133 87 | Default Light: true 88 | Fixed Frame: map 89 | Frame Rate: 30 90 | Name: root 91 | Tools: 92 | - Class: rviz/Interact 93 | Hide Inactive Objects: true 94 | - Class: rviz/MoveCamera 95 | - Class: rviz/Select 96 | - Class: rviz/FocusCamera 97 | - Class: rviz/Measure 98 | - Class: rviz/SetInitialPose 99 | Theta std deviation: 0.2617993950843811 100 | Topic: /initialpose 101 | X std deviation: 0.5 102 | Y std deviation: 0.5 103 | - Class: rviz/SetGoal 104 | Topic: /move_base_simple/goal 105 | - Class: rviz/PublishPoint 106 | Single click: true 107 | Topic: /clicked_point 108 | Value: true 109 | Views: 110 | Current: 111 | Class: rviz/Orbit 112 | Distance: 6.43789005279541 113 | Enable Stereo Rendering: 114 | Stereo Eye Separation: 0.05999999865889549 115 | Stereo Focal Distance: 1 116 | Swap Stereo Eyes: false 117 | Value: false 118 | Field of View: 0.7853981852531433 119 | Focal Point: 120 | X: 2.4363977909088135 121 | Y: 2.436851739883423 122 | Z: 0.12684479355812073 123 | Focal Shape Fixed Size: true 124 | Focal Shape Size: 0.05000000074505806 125 | Invert Z Axis: false 126 | Name: Current View 127 | Near Clip Distance: 0.009999999776482582 128 | Pitch: 1.5697963237762451 129 | Target Frame: 130 | Yaw: 4.710451126098633 131 | Saved: ~ 132 | Window Geometry: 133 | Displays: 134 | collapsed: false 135 | Height: 1016 136 | Hide Left Dock: false 137 | Hide Right Dock: true 138 | OMPL ControlPanel: 139 | collapsed: false 140 | QMainWindow State: 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 141 | Selection: 142 | collapsed: false 143 | Time: 144 | collapsed: false 145 | Tool Properties: 146 | collapsed: false 147 | Views: 148 | collapsed: true 149 | Width: 1856 150 | X: 64 151 | Y: 27 152 | -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/include/ompl_2d_rviz_visualizer_ros/collision_checker.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | BSD 2-Clause License 3 | 4 | Copyright (c) 2021, Phone Thiha Kyaw 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 are met: 9 | 10 | 1. Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | 2. Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 21 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 23 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 24 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 25 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | ******************************************************************************/ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | namespace ob = ompl::base; 38 | 39 | namespace ompl_2d_rviz_visualizer_ros 40 | { 41 | class CollisionChecker 42 | { 43 | public: 44 | CollisionChecker(std::shared_ptr ogm_map) : map_{ std::move(ogm_map) } 45 | { 46 | if (!map_utils::getBounds(min_bound_x_, max_bound_x_, min_bound_y_, max_bound_y_, *map_)) 47 | { 48 | std::cerr << "Fail to generate bounds in the occupancy grid map." << std::endl; 49 | exit(1); 50 | } 51 | 52 | map_cell_size_x_ = map_->info.width; 53 | map_cell_size_y_ = map_->info.height; 54 | map_resolution_ = map_->info.resolution; 55 | map_origin_x_ = map_->info.origin.position.x; 56 | map_origin_y_ = map_->info.origin.position.y; 57 | } 58 | 59 | ~CollisionChecker(); 60 | 61 | bool isValid(const ob::State* state) 62 | { 63 | if (!state) 64 | { 65 | throw std::runtime_error("No state found for vertex"); 66 | } 67 | 68 | // Convert to RealVectorStateSpace 69 | const ob::RealVectorStateSpace::StateType* real_state = 70 | static_cast(state); 71 | 72 | double x = real_state->values[0]; 73 | double y = real_state->values[1]; 74 | 75 | return isValid(x, y); 76 | } 77 | 78 | bool isValid(double x, double y) 79 | { 80 | // find cell index in the map 81 | unsigned int idx = mapToCellIndex(x, y); 82 | auto occ = map_->data[idx]; 83 | 84 | if (occ == OCC_GRID_FREE) 85 | return true; 86 | return false; 87 | } 88 | 89 | private: 90 | unsigned int mapToCellIndex(double x, double y) 91 | { 92 | // check if the state is within bounds 93 | if (!(x >= min_bound_x_ && x <= max_bound_x_ && y >= min_bound_y_ && y <= max_bound_y_)) 94 | { 95 | throw std::runtime_error("State must be within the bounds."); 96 | } 97 | 98 | int cell_x = (int)((x - map_origin_x_) / map_resolution_); 99 | int cell_y = (int)((y - map_origin_y_) / map_resolution_); 100 | 101 | unsigned int idx = cell_y * map_cell_size_x_ + cell_x; 102 | return idx; 103 | } 104 | 105 | std::shared_ptr map_; 106 | 107 | double min_bound_x_; 108 | double min_bound_y_; 109 | double max_bound_x_; 110 | double max_bound_y_; 111 | 112 | unsigned int map_cell_size_x_; 113 | unsigned int map_cell_size_y_; 114 | double map_resolution_; 115 | double map_origin_x_; 116 | double map_origin_y_; 117 | }; 118 | 119 | } // namespace ompl_2d_rviz_visualizer_ros -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/include/ompl_2d_rviz_visualizer_ros/map_loader.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Rover Robotics 2 | * Copyright 2010 Brian Gerkey 3 | * Copyright (c) 2008, Willow Garage, Inc. 4 | * 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 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 copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | * Modified by: Phone Thiha Kyaw, Sai Htet Moe Swe 32 | */ 33 | 34 | #pragma once 35 | 36 | #define MAPLOADER_DEBUG 0 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | 45 | #include "ros/ros.h" 46 | #include "yaml-cpp/yaml.h" 47 | 48 | namespace ompl_2d_rviz_visualizer_ros 49 | { 50 | enum MapMode 51 | { 52 | TRINARY, 53 | SCALE, 54 | RAW 55 | }; 56 | 57 | struct LoadParameters 58 | { 59 | std::string image_file_name; 60 | double resolution{ 0 }; 61 | std::vector origin{ 0, 0, 0 }; 62 | double free_thresh; 63 | double occupied_thresh; 64 | MapMode mode; 65 | bool negate; 66 | }; 67 | 68 | class MapLoader 69 | { 70 | public: 71 | MapLoader(const std::string& map_frame_name, const std::string& map_topic_name, 72 | ros::NodeHandle nh = ros::NodeHandle("~")); 73 | 74 | ~MapLoader(); 75 | 76 | bool loadMapFromYaml(const std::string& path_to_yaml, nav_msgs::OccupancyGrid& ogm_map); 77 | 78 | private: 79 | template 80 | T yamlGetValue(const YAML::Node& node, const std::string& key) 81 | { 82 | try 83 | { 84 | return node[key].as(); 85 | } 86 | catch (YAML::Exception& e) 87 | { 88 | std::stringstream ss; 89 | ss << "Failed to parse YAML tag '" << key << "' for reason: " << e.msg; 90 | throw YAML::Exception(e.mark, ss.str()); 91 | } 92 | } 93 | 94 | bool loadMapYaml(LoadParameters& load_parameters, const std::string& path_to_yaml); 95 | 96 | void loadMapFromFile(nav_msgs::OccupancyGrid& map, const LoadParameters& load_parameters); 97 | 98 | const char* mapModeToString(MapMode map_mode); 99 | MapMode mapModeFromString(std::string map_mode_name); 100 | 101 | ros::NodeHandle nh_; 102 | ros::Publisher map_pub_; 103 | std::string map_frame_name_; 104 | std::string map_topic_name_; 105 | }; 106 | } // namespace ompl_2d_rviz_visualizer_ros 107 | -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/include/ompl_2d_rviz_visualizer_ros/map_utils.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | BSD 2-Clause License 3 | 4 | Copyright (c) 2021, Phone Thiha Kyaw 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 are met: 9 | 10 | 1. Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | 2. Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 21 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 23 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 24 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 25 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | ******************************************************************************/ 28 | 29 | #pragma once 30 | 31 | #define MAP_UTILS_DEBUG 0 32 | 33 | #include 34 | 35 | namespace ompl_2d_rviz_visualizer_ros 36 | { 37 | /** 38 | * @brief OccupancyGrid data constants 39 | */ 40 | static constexpr int8_t OCC_GRID_UNKNOWN = -1; 41 | static constexpr int8_t OCC_GRID_FREE = 0; 42 | static constexpr int8_t OCC_GRID_OCCUPIED = 100; 43 | 44 | namespace map_utils 45 | { 46 | /** 47 | * @brief Generate realvector bounds of an 2D occupancy grid map 48 | * @param min_x Lower map bound in x-axis 49 | * @param max_x Higher map bound in x-axis 50 | * @param min_y Lower map bound in y-axis 51 | * @param max_y Higher map bound in y-axis 52 | * @return true if can generate bounds 53 | */ 54 | inline bool getBounds(double& min_x, double& max_x, double& min_y, double& max_y, const nav_msgs::OccupancyGrid& ogm) 55 | { 56 | // extract map parameters 57 | unsigned int cells_size_x = ogm.info.width; 58 | unsigned int cells_size_y = ogm.info.height; 59 | double resolution = static_cast(ogm.info.resolution); 60 | double origin_x = ogm.info.origin.position.x; 61 | double origin_y = ogm.info.origin.position.y; 62 | 63 | double map_size_x = cells_size_x * resolution; 64 | double map_size_y = cells_size_y * resolution; 65 | 66 | min_x = origin_x; 67 | min_y = origin_y; 68 | max_x = map_size_x - fabs(origin_x); 69 | max_y = map_size_y - fabs(origin_y); 70 | 71 | #if MAP_UTILS_DEBUG 72 | std::cout << "[DEBUG] [map_utils] Map size in meters: " << map_size_x << " X " << map_size_y << std::endl; 73 | std::cout << "[DEBUG] [map_utils] Map bounds (lower-left): " << min_x << ", " << min_y << std::endl; 74 | std::cout << "[DEBUG] [map_utils] Map bounds (upper-right): " << max_x << ", " << max_y << std::endl; 75 | #endif 76 | 77 | return true; 78 | } 79 | 80 | } // namespace map_utils 81 | } // namespace ompl_2d_rviz_visualizer_ros -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/include/ompl_2d_rviz_visualizer_ros/rviz_renderer.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | BSD 2-Clause License 3 | 4 | Copyright (c) 2021, Phone Thiha Kyaw 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 are met: 9 | 10 | 1. Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | 2. Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 21 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 23 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 24 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 25 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | ******************************************************************************/ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | namespace ob = ompl::base; 41 | namespace og = ompl::geometric; 42 | namespace rvt = rviz_visual_tools; 43 | 44 | namespace ompl_2d_rviz_visualizer_ros 45 | { 46 | class RvizRenderer 47 | { 48 | public: 49 | /** 50 | * @brief Constructor 51 | * @param base_frame common base for all visualization markers, usually 52 | * "/world" or "/odom" 53 | * @param marker_topic rostopic to publish markers on Rviz 54 | * @param nh optional ros node handle (default: "~") 55 | */ 56 | RvizRenderer(const std::string& base_frame, const std::string& marker_topic, 57 | ros::NodeHandle nh = ros::NodeHandle("~")); 58 | 59 | /** 60 | * @brief Destructor 61 | */ 62 | ~RvizRenderer(); 63 | 64 | /** 65 | * @brief Clear all the published markers on Rviz. 66 | */ 67 | bool deleteAllMarkers(); 68 | 69 | /** 70 | * @brief Clear all the published markers on Rviz within specific namespace. 71 | */ 72 | bool deleteAllMarkersInNS(const std::string& ns); 73 | 74 | bool deleteMarkerInNSAndID(const std::string& ns, std::size_t id); 75 | 76 | /** 77 | * @brief Render state on rviz. State is basically a sphere with adjustable 78 | * size and color. 79 | * @param state ompl abstract state 80 | * @param color rviz_visual_tools color 81 | * @param scale rviz_visual_tools scale 82 | * @param ns namespace of marker 83 | * @param id marker id 84 | */ 85 | bool renderState(const ob::State* state, const rvt::colors& color, const rvt::scales& scale, const std::string& ns, 86 | std::size_t id = 0); 87 | 88 | /** 89 | * @brief Render state on rviz. State is basically a sphere with adjustable 90 | * size and color. 91 | * @param point eigen vector3D 92 | * @param color rviz_visual_tools color 93 | * @param scale rviz_visual_tools scale 94 | * @param ns namespace of marker 95 | * @param id marker id 96 | */ 97 | bool renderState(const Eigen::Vector3d& point, const rvt::colors& color, const rvt::scales& scale, 98 | const std::string& ns, std::size_t id = 0); 99 | 100 | /** 101 | * @brief Render 2D geometric path on rviz. Path is basically a cylinder with 102 | * adjustable radius and color. 103 | * @param path ompl geometric 2D path 104 | * @param color rviz_visual_tools color 105 | * @param radius geometry of cylinder 106 | * @param ns namespace of marker 107 | */ 108 | bool renderPath(const og::PathGeometric& path, const rvt::colors& color, const double radius, const std::string& ns); 109 | 110 | /** 111 | * @brief Render ompl planner data graph on rviz. 112 | * @param planner_data ompl planner data 113 | * @param color rviz_visual_tools color 114 | * @param radius geometry of cylinder 115 | * @param ns namespace of marker 116 | */ 117 | bool renderGraph(const ob::PlannerDataPtr planner_data, const rvt::colors& color, const double radius, 118 | const std::string& ns); 119 | 120 | private: 121 | /** 122 | * @brief Store marker namespace and id to be used later. 123 | * @param ns namespace of marker 124 | * @param id marker id 125 | */ 126 | void addToMarkerIDs(const std::string& ns, std::size_t id); 127 | 128 | /** 129 | * @brief Publish cylinder marker to Rviz. 130 | * @param point1 eigen vector3D point 131 | * @param point2 eigen vector3D point 132 | * @param color rviz_visual_tools color 133 | * @param radius geometry of cylinder 134 | * @param ns namespace of marker 135 | * @param id marker id 136 | */ 137 | bool publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const rvt::colors& color, 138 | const double radius, const std::string& ns, std::size_t id); 139 | 140 | /** 141 | * @brief Convert ompl abstract state to eigen vector3D. 142 | * @param state ompl abstract state 143 | * @return eigen vector3D 144 | */ 145 | Eigen::Vector3d stateToPoint(const ob::State* state); 146 | 147 | /** 148 | * @brief Convert ompl abstract state to ros message. 149 | * @param state ompl abstract state 150 | * @return geometry_msgs/Point 151 | */ 152 | geometry_msgs::Point stateToPointMsg(const ob::State* state); 153 | 154 | std::string base_frame_; 155 | rvt::RvizVisualToolsPtr visual_tools_; 156 | 157 | // we will store all the published namespace and ids 158 | // these will be useful for clearing specific marker 159 | std::unordered_map> marker_ids_; 160 | }; 161 | 162 | typedef std::shared_ptr RvizRendererPtr; 163 | typedef std::shared_ptr RvizRendererConstPtr; 164 | 165 | } // namespace ompl_2d_rviz_visualizer_ros -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/launch/visualizer.launch: -------------------------------------------------------------------------------- 1 | 2 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/map/empty_map/empty_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/ompl_2d_rviz_visualizer/39ea31dbab97eec023d29b27f24bab2286c3370a/ompl_2d_rviz_visualizer_ros/map/empty_map/empty_map.pgm -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/map/empty_map/empty_map.yaml: -------------------------------------------------------------------------------- 1 | image: ./empty_map.pgm 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/map/map_with_many_homotopy_classes/map_with_many_homotopy_classes.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/ompl_2d_rviz_visualizer/39ea31dbab97eec023d29b27f24bab2286c3370a/ompl_2d_rviz_visualizer_ros/map/map_with_many_homotopy_classes/map_with_many_homotopy_classes.pgm -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/map/map_with_many_homotopy_classes/map_with_many_homotopy_classes.yaml: -------------------------------------------------------------------------------- 1 | image: ./map_with_many_homotopy_classes.pgm 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/map/map_with_single_cube/map_with_single_cube.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/ompl_2d_rviz_visualizer/39ea31dbab97eec023d29b27f24bab2286c3370a/ompl_2d_rviz_visualizer_ros/map/map_with_single_cube/map_with_single_cube.pgm -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/map/map_with_single_cube/map_with_single_cube.yaml: -------------------------------------------------------------------------------- 1 | image: ./map_with_single_cube.pgm 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/map/map_with_single_narrow_passage_gap/map_with_single_narrow_passage_gap.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/ompl_2d_rviz_visualizer/39ea31dbab97eec023d29b27f24bab2286c3370a/ompl_2d_rviz_visualizer_ros/map/map_with_single_narrow_passage_gap/map_with_single_narrow_passage_gap.pgm -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/map/map_with_single_narrow_passage_gap/map_with_single_narrow_passage_gap.yaml: -------------------------------------------------------------------------------- 1 | image: ./map_with_single_narrow_passage_gap.pgm 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ompl_2d_rviz_visualizer_ros 4 | 1.0.0 5 | Plugin for visualizing ompl motion planning algorithms in 2D 6 | 7 | Phone Thiha Kyaw 8 | 9 | TODO 10 | 11 | Phone Thiha Kyaw 12 | 13 | catkin 14 | 15 | roscpp 16 | nodelet 17 | ompl 18 | eigen_conversions 19 | rviz 20 | rviz_visual_tools 21 | ompl_2d_rviz_visualizer_msgs 22 | nav_msgs 23 | sdl 24 | sdl-image 25 | yaml-cpp 26 | bullet 27 | tf2 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A panel widget allowing control of the MoveIt pipeline 7 | 8 | 9 | -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/src/map_loader.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Rover Robotics 2 | * Copyright 2010 Brian Gerkey 3 | * Copyright (c) 2008, Willow Garage, Inc. 4 | * 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 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 copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | * Modified by: Phone Thiha Kyaw, Sai Htet Moe Swe 32 | */ 33 | 34 | #include 35 | 36 | namespace ompl_2d_rviz_visualizer_ros 37 | { 38 | MapLoader::MapLoader(const std::string& map_frame_name, const std::string& map_topic_name, ros::NodeHandle nh) 39 | : map_frame_name_{ map_frame_name }, map_topic_name_{ map_topic_name }, nh_{ nh } 40 | { 41 | map_pub_ = nh_.advertise(map_topic_name, 1, true); 42 | } 43 | 44 | MapLoader::~MapLoader() 45 | { 46 | } 47 | 48 | bool MapLoader::loadMapFromYaml(const std::string& path_to_yaml, nav_msgs::OccupancyGrid& map) 49 | { 50 | // first check path is empty or not 51 | if (path_to_yaml.empty()) 52 | { 53 | std::cerr << "[ERROR] [map_loader]: YAML file name is empty, can't load!" << std::endl; 54 | return false; 55 | } 56 | 57 | std::cout << "[ INFO] [map_loader]: Loading yaml file: " << path_to_yaml << std::endl; 58 | 59 | LoadParameters load_parameters; 60 | try 61 | { 62 | loadMapYaml(load_parameters, path_to_yaml); 63 | } 64 | catch (YAML::Exception& e) 65 | { 66 | std::cerr << "[ERROR] [map_loader]: Failed processing YAML file " << path_to_yaml << " at position (" << e.mark.line 67 | << ":" << e.mark.column << ") for reason: " << e.what() << std::endl; 68 | return false; 69 | } 70 | catch (std::exception& e) 71 | { 72 | std::cerr << "[ERROR] [map_loader]: Failed to parse map YAML loaded from file " << path_to_yaml 73 | << " for reason: " << e.what() << std::endl; 74 | return false; 75 | } 76 | 77 | try 78 | { 79 | loadMapFromFile(map, load_parameters); 80 | } 81 | catch (std::exception& e) 82 | { 83 | std::cerr << "[ERROR] [map_io]: Failed to load image file " << load_parameters.image_file_name 84 | << " for reason: " << e.what() << std::endl; 85 | return false; 86 | } 87 | 88 | return true; 89 | } 90 | 91 | bool MapLoader::loadMapYaml(LoadParameters& load_parameters, const std::string& path_to_yaml) 92 | { 93 | YAML::Node doc = YAML::LoadFile(path_to_yaml); 94 | 95 | auto image_file_name = yamlGetValue(doc, "image"); 96 | if (image_file_name.empty()) 97 | { 98 | throw YAML::Exception(doc["image"].Mark(), "The image tag was empty."); 99 | } 100 | 101 | boost::filesystem::path mapfpath(image_file_name); 102 | if (!mapfpath.is_absolute()) 103 | { 104 | boost::filesystem::path dir(path_to_yaml); 105 | dir = dir.parent_path(); 106 | mapfpath = dir / mapfpath; 107 | image_file_name = mapfpath.string(); 108 | } 109 | 110 | load_parameters.image_file_name = image_file_name; 111 | 112 | load_parameters.resolution = yamlGetValue(doc, "resolution"); 113 | load_parameters.origin = yamlGetValue>(doc, "origin"); 114 | if (load_parameters.origin.size() != 3) 115 | { 116 | throw YAML::Exception(doc["origin"].Mark(), "value of the 'origin' tag should have 3 elements, not " + 117 | std::to_string(load_parameters.origin.size())); 118 | } 119 | 120 | load_parameters.free_thresh = yamlGetValue(doc, "free_thresh"); 121 | load_parameters.occupied_thresh = yamlGetValue(doc, "occupied_thresh"); 122 | 123 | auto map_mode_node = doc["mode"]; 124 | if (!map_mode_node.IsDefined()) 125 | { 126 | load_parameters.mode = MapMode::TRINARY; 127 | } 128 | else 129 | { 130 | load_parameters.mode = mapModeFromString(map_mode_node.as()); 131 | } 132 | 133 | try 134 | { 135 | load_parameters.negate = yamlGetValue(doc, "negate"); 136 | } 137 | catch (YAML::Exception&) 138 | { 139 | load_parameters.negate = yamlGetValue(doc, "negate"); 140 | } 141 | 142 | #if MAPLOADER_DEBUG 143 | std::cout << "[DEBUG] [map_loader]: resolution: " << load_parameters.resolution << std::endl; 144 | std::cout << "[DEBUG] [map_loader]: origin[0]: " << load_parameters.origin[0] << std::endl; 145 | std::cout << "[DEBUG] [map_loader]: origin[1]: " << load_parameters.origin[1] << std::endl; 146 | std::cout << "[DEBUG] [map_loader]: origin[2]: " << load_parameters.origin[2] << std::endl; 147 | std::cout << "[DEBUG] [map_loader]: free_thresh: " << load_parameters.free_thresh << std::endl; 148 | std::cout << "[DEBUG] [map_loader]: occupied_thresh: " << load_parameters.occupied_thresh << std::endl; 149 | std::cout << "[DEBUG] [map_loader]: mode: " << mapModeToString(load_parameters.mode) << std::endl; 150 | std::cout << "[DEBUG] [map_loader]: negate: " << load_parameters.negate << std::endl; // NOLINT 151 | #endif 152 | 153 | return true; 154 | } 155 | 156 | void MapLoader::loadMapFromFile(nav_msgs::OccupancyGrid& map, const LoadParameters& load_parameters) 157 | { 158 | SDL_Surface* img; 159 | 160 | // Load the image using SDL. If we get NULL back, the image load failed. 161 | if (!(img = IMG_Load(load_parameters.image_file_name.c_str()))) 162 | { 163 | std::string errmsg = std::string("failed to open image file \"") + 164 | std::string(load_parameters.image_file_name.c_str()) + std::string("\": ") + IMG_GetError(); 165 | throw std::runtime_error(errmsg); 166 | } 167 | 168 | nav_msgs::OccupancyGrid msg; 169 | msg.info.width = img->w; 170 | msg.info.height = img->h; 171 | msg.info.resolution = static_cast(load_parameters.resolution); 172 | msg.info.origin.position.x = load_parameters.origin[0]; 173 | msg.info.origin.position.y = load_parameters.origin[1]; 174 | msg.info.origin.position.z = 0.0; 175 | 176 | tf2::Quaternion q; 177 | q.setRPY(0, 0, load_parameters.origin[2]); 178 | msg.info.origin.orientation = tf2::toMsg(q); 179 | 180 | // Allocate space to hold the data 181 | msg.data.resize(msg.info.width * msg.info.height); 182 | 183 | // Get values that we'll need to iterate through the pixels 184 | unsigned int rowstride, n_channels, avg_channels; 185 | rowstride = img->pitch; 186 | n_channels = img->format->BytesPerPixel; 187 | 188 | // NOTE: Trinary mode still overrides here to preserve existing behavior. 189 | // Alpha will be averaged in with color channels when using trinary mode. 190 | if (load_parameters.mode == MapMode::TRINARY || !img->format->Amask) 191 | avg_channels = n_channels; 192 | else 193 | avg_channels = n_channels - 1; 194 | 195 | // Copy pixel data into the map structure 196 | unsigned char *pixels, *p; 197 | int color_sum, alpha; 198 | double occ; 199 | pixels = (unsigned char*)(img->pixels); 200 | for (auto j = 0; j < msg.info.height; j++) 201 | { 202 | for (auto i = 0; i < msg.info.width; i++) 203 | { 204 | // Compute mean of RGB for this pixel 205 | p = pixels + j * rowstride + i * n_channels; 206 | color_sum = 0; 207 | for (auto k = 0; k < avg_channels; k++) 208 | color_sum += *(p + (k)); 209 | occ = color_sum / static_cast(avg_channels); 210 | 211 | if (n_channels == 1) 212 | alpha = 1; 213 | else 214 | alpha = *(p + n_channels - 1); 215 | 216 | if (load_parameters.negate) 217 | occ = 255.0 - occ; 218 | 219 | int8_t map_cell; 220 | switch (load_parameters.mode) 221 | { 222 | case MapMode::RAW: 223 | if (OCC_GRID_FREE <= occ && occ <= OCC_GRID_OCCUPIED) 224 | { 225 | map_cell = static_cast(occ); 226 | } 227 | else 228 | { 229 | map_cell = OCC_GRID_UNKNOWN; 230 | } 231 | break; 232 | 233 | case MapMode::TRINARY: 234 | // If negate is true, we consider blacker pixels free, and whiter 235 | // pixels occupied. Otherwise, it's vice versa. 236 | occ = (255 - occ) / 255.0; 237 | if (load_parameters.occupied_thresh < occ) 238 | { 239 | map_cell = OCC_GRID_OCCUPIED; 240 | } 241 | else if (occ < load_parameters.free_thresh) 242 | { 243 | map_cell = OCC_GRID_FREE; 244 | } 245 | else 246 | { 247 | map_cell = OCC_GRID_UNKNOWN; 248 | } 249 | break; 250 | 251 | case MapMode::SCALE: 252 | occ = (255 - occ) / 255.0; 253 | if (alpha < 1.0) 254 | { 255 | map_cell = OCC_GRID_UNKNOWN; 256 | } 257 | else if (load_parameters.occupied_thresh < occ) 258 | { 259 | map_cell = OCC_GRID_OCCUPIED; 260 | } 261 | else if (occ < load_parameters.free_thresh) 262 | { 263 | map_cell = OCC_GRID_FREE; 264 | } 265 | else 266 | { 267 | map_cell = std::rint((occ - load_parameters.free_thresh) / 268 | (load_parameters.occupied_thresh - load_parameters.free_thresh) * 100.0); 269 | } 270 | break; 271 | 272 | default: 273 | throw std::runtime_error("Invalid map mode"); 274 | } 275 | msg.data[msg.info.width * (msg.info.height - j - 1) + i] = map_cell; 276 | } 277 | } 278 | SDL_FreeSurface(img); 279 | 280 | ros::Time::waitForValid(); 281 | msg.info.map_load_time = ros::Time::now(); 282 | msg.header.frame_id = map_frame_name_; 283 | msg.header.stamp = ros::Time::now(); 284 | 285 | #if MAPLOADER_DEBUG 286 | std::cout << "[DEBUG] [map_loader]: Read map " << load_parameters.image_file_name << ": " << msg.info.width << " X " 287 | << msg.info.height << " map @ " << msg.info.resolution << " m/cell" << std::endl; 288 | #endif 289 | 290 | map = msg; 291 | map_pub_.publish(msg); 292 | } 293 | 294 | const char* MapLoader::mapModeToString(MapMode map_mode) 295 | { 296 | switch (map_mode) 297 | { 298 | case MapMode::TRINARY: 299 | return "trinary"; 300 | case MapMode::SCALE: 301 | return "scale"; 302 | case MapMode::RAW: 303 | return "raw"; 304 | default: 305 | throw std::invalid_argument("map_mode"); 306 | } 307 | } 308 | 309 | MapMode MapLoader::mapModeFromString(std::string map_mode_name) 310 | { 311 | for (auto& c : map_mode_name) 312 | { 313 | c = tolower(c); 314 | } 315 | 316 | if (map_mode_name == "scale") 317 | { 318 | return MapMode::SCALE; 319 | } 320 | else if (map_mode_name == "raw") 321 | { 322 | return MapMode::RAW; 323 | } 324 | else if (map_mode_name == "trinary") 325 | { 326 | return MapMode::TRINARY; 327 | } 328 | else 329 | { 330 | throw std::invalid_argument("map_mode_name"); 331 | } 332 | } 333 | 334 | } // namespace ompl_2d_rviz_visualizer_ros -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/src/rviz_control_panel.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | BSD 2-Clause License 3 | 4 | Copyright (c) 2021, Phone Thiha Kyaw 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 are met: 9 | 10 | 1. Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | 2. Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 21 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 23 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 24 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 25 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | ******************************************************************************/ 28 | 29 | #include "rviz_control_panel.h" 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace ompl_2d_rviz_visualizer_ros 37 | { 38 | OMPL_ControlPanel::OMPL_ControlPanel(QWidget* parent) 39 | : rviz::Panel(parent) 40 | , nh_{ "ompl_2d_rviz_visualizer_nodelet" } 41 | , prv_nh_{ "~ompl_controlpanel" } 42 | , planner_id_{ PLANNERS_IDS::INVALID } 43 | , planning_obj_id_{ PLANNING_OBJS_IDS::PATH_LENGTH } 44 | , planning_mode_{ PLANNING_MODE::DURATION } 45 | { 46 | //////////////////// 47 | // Start layout 48 | //////////////////// 49 | QHBoxLayout* start_hlayout = new QHBoxLayout; 50 | 51 | start_check_box_ = new QRadioButton("Start", this); 52 | connect(start_check_box_, SIGNAL(toggled(bool)), this, SLOT(startCheckBoxStateChanged(bool))); 53 | 54 | start_combo_box_ = new QComboBox(this); 55 | start_combo_box_->addItems(START_GOAL_COMBO_BOX_ITEMS); 56 | connect(start_combo_box_, SIGNAL(activated(int)), this, SLOT(startComboBoxActivated(int))); 57 | 58 | start_x_spin_box_ = new QDoubleSpinBox(this); 59 | QHBoxLayout* start_x_hlayout = new QHBoxLayout; 60 | start_x_hlayout->addWidget(new QLabel(QString("X:"))); 61 | start_x_hlayout->addWidget(start_x_spin_box_); 62 | 63 | start_y_spin_box_ = new QDoubleSpinBox(this); 64 | QHBoxLayout* start_y_hlayout = new QHBoxLayout; 65 | start_y_hlayout->addWidget(new QLabel(QString("Y:"))); 66 | start_y_hlayout->addWidget(start_y_spin_box_); 67 | 68 | btn_start_ = new QPushButton(this); 69 | btn_start_->setText("Send"); 70 | connect(btn_start_, SIGNAL(clicked()), this, SLOT(btn_start_clicked())); 71 | 72 | start_hlayout->addWidget(start_check_box_); 73 | start_hlayout->addWidget(start_combo_box_); 74 | start_hlayout->addLayout(start_x_hlayout); 75 | start_hlayout->addLayout(start_y_hlayout); 76 | start_hlayout->addWidget(btn_start_); 77 | 78 | start_check_box_->setChecked(true); 79 | start_combo_box_->setEnabled(false); 80 | start_x_spin_box_->setEnabled(false); 81 | start_y_spin_box_->setEnabled(false); 82 | btn_start_->setEnabled(false); 83 | //////////////////// 84 | 85 | //////////////////// 86 | // Goal layout 87 | //////////////////// 88 | QHBoxLayout* goal_hlayout = new QHBoxLayout; 89 | 90 | goal_check_box_ = new QRadioButton("Goal", this); 91 | connect(goal_check_box_, SIGNAL(toggled(bool)), this, SLOT(goalCheckBoxStateChanged(bool))); 92 | 93 | goal_combo_box_ = new QComboBox(this); 94 | goal_combo_box_->addItems(START_GOAL_COMBO_BOX_ITEMS); 95 | connect(goal_combo_box_, SIGNAL(activated(int)), this, SLOT(goalComboBoxActivated(int))); 96 | 97 | goal_x_spin_box_ = new QDoubleSpinBox(this); 98 | QHBoxLayout* goal_x_hlayout = new QHBoxLayout; 99 | goal_x_hlayout->addWidget(new QLabel(QString("X:"))); 100 | goal_x_hlayout->addWidget(goal_x_spin_box_); 101 | 102 | goal_y_spin_box_ = new QDoubleSpinBox(this); 103 | QHBoxLayout* goal_y_hlayout = new QHBoxLayout; 104 | goal_y_hlayout->addWidget(new QLabel(QString("Y:"))); 105 | goal_y_hlayout->addWidget(goal_y_spin_box_); 106 | 107 | btn_goal_ = new QPushButton(this); 108 | btn_goal_->setText("Send"); 109 | connect(btn_goal_, SIGNAL(clicked()), this, SLOT(btn_goal_clicked())); 110 | 111 | goal_hlayout->addWidget(goal_check_box_); 112 | goal_hlayout->addWidget(goal_combo_box_); 113 | goal_hlayout->addLayout(goal_x_hlayout); 114 | goal_hlayout->addLayout(goal_y_hlayout); 115 | goal_hlayout->addWidget(btn_goal_); 116 | 117 | goal_combo_box_->setEnabled(false); 118 | goal_x_spin_box_->setEnabled(false); 119 | goal_y_spin_box_->setEnabled(false); 120 | btn_goal_->setEnabled(false); 121 | //////////////////// 122 | 123 | ///////////////////////////// 124 | // Start and Goal Group Box 125 | ///////////////////////////// 126 | QGroupBox* query_gp_box = new QGroupBox(QString("Query")); 127 | QVBoxLayout* query_v_layout = new QVBoxLayout; 128 | query_v_layout->addLayout(start_hlayout); 129 | query_v_layout->addLayout(goal_hlayout); 130 | query_gp_box->setLayout(query_v_layout); 131 | ///////////////////////////// 132 | 133 | //////////////////////////////////////// 134 | // Planner related layout 135 | //////////////////////////////////////// 136 | planner_combo_box_ = new QComboBox; 137 | planner_combo_box_->addItems(PLANNERS); 138 | connect(planner_combo_box_, SIGNAL(activated(int)), this, SLOT(plannerComboBoxActivated(int))); 139 | 140 | QScrollArea* scroll_area = new QScrollArea; 141 | scroll_area->setWidgetResizable(true); 142 | QWidget* temp = new QWidget; 143 | scroll_area->setWidget(temp); 144 | planner_params_v_layout_ = new QVBoxLayout; 145 | temp->setLayout(planner_params_v_layout_); 146 | 147 | QVBoxLayout* planner_vlayout = new QVBoxLayout; 148 | planner_vlayout->addWidget(planner_combo_box_); 149 | planner_vlayout->addWidget(new QLabel(QString("Parameters"))); 150 | planner_vlayout->addWidget(scroll_area); 151 | 152 | QGroupBox* planner_gp_box = new QGroupBox(QString("Planner")); 153 | planner_gp_box->setLayout(planner_vlayout); 154 | 155 | //////////////////////////////////////// 156 | 157 | ///////////////////////////////////////// 158 | // Planning objective 159 | ///////////////////////////////////////// 160 | planning_objective_combo_box_ = new QComboBox; 161 | planning_objective_combo_box_->addItems(PLANNING_OBJS); 162 | connect(planning_objective_combo_box_, SIGNAL(activated(int)), this, SLOT(planningObjectiveComboBoxActivated(int))); 163 | 164 | QHBoxLayout* planning_obj_hlayout = new QHBoxLayout; 165 | planning_obj_hlayout->addWidget(planning_objective_combo_box_); 166 | 167 | QGroupBox* planning_obj_gp_box = new QGroupBox(QString("Planning objective")); 168 | planning_obj_gp_box->setLayout(planning_obj_hlayout); 169 | 170 | ///////////////////////////////////////// 171 | 172 | /////////////////// 173 | // Animation mode 174 | /////////////////// 175 | animation_mode_check_box_ = new QCheckBox("Show animation"); 176 | connect(animation_mode_check_box_, SIGNAL(stateChanged(int)), this, SLOT(animationModeCheckBoxStateChanged(int))); 177 | 178 | animate_every_spin_box_ = new QSpinBox; 179 | animate_every_spin_box_->setMinimumWidth(100); 180 | 181 | // TODO: this range must be set as static const 182 | animate_every_spin_box_->setMinimum(1); 183 | animate_every_spin_box_->setMaximum(1000000); 184 | 185 | animation_speed_slider_ = new QSlider(Qt::Orientation::Horizontal); 186 | animation_speed_slider_->setMinimum(1); 187 | animation_speed_slider_->setMaximum(100); 188 | 189 | QHBoxLayout* animation_hlayout = new QHBoxLayout; 190 | animation_hlayout->addWidget(animation_mode_check_box_); 191 | 192 | animation_hlayout->addWidget(new QLabel(QString("animate_every:"))); 193 | animation_hlayout->addWidget(animate_every_spin_box_); 194 | 195 | animation_hlayout->addWidget(new QLabel(QString("animation_speed:"))); 196 | animation_hlayout->addWidget(animation_speed_slider_); 197 | 198 | QGroupBox* animation_gp_box = new QGroupBox(QString("Animation Mode")); 199 | animation_gp_box->setLayout(animation_hlayout); 200 | 201 | animate_every_spin_box_->setEnabled(false); 202 | animation_speed_slider_->setEnabled(false); 203 | /////////////////// 204 | 205 | ///////////////////////////////////////// 206 | // Termination conditions 207 | ///////////////////////////////////////// 208 | ptc_combo_box_ = new QComboBox; 209 | ptc_combo_box_->addItems(PTCS); 210 | connect(ptc_combo_box_, SIGNAL(activated(int)), this, SLOT(ptcComboBoxActivated(int))); 211 | 212 | planning_duration_spin_box_ = new QDoubleSpinBox; 213 | planning_duration_spin_box_->setMinimum(0.0); 214 | planning_duration_spin_box_->setSingleStep(0.1); 215 | planning_duration_spin_box_->setMinimumWidth(50); 216 | 217 | ptc_iteration_number_spin_box_ = new QSpinBox; 218 | ptc_iteration_number_spin_box_->setMinimumWidth(100); 219 | // TODO: this range must be set as static const 220 | ptc_iteration_number_spin_box_->setMinimum(1); 221 | ptc_iteration_number_spin_box_->setMaximum(1000000); 222 | 223 | QHBoxLayout* ptc_hlayout = new QHBoxLayout; 224 | ptc_hlayout->addWidget(ptc_combo_box_); 225 | QLabel* duration_label = new QLabel(QString("duration:")); 226 | duration_label->setAlignment(Qt::AlignVCenter | Qt::AlignRight); 227 | ptc_hlayout->addWidget(duration_label); 228 | ptc_hlayout->addWidget(planning_duration_spin_box_); 229 | QLabel* iter_no_label = new QLabel(QString("iteration number:")); 230 | iter_no_label->setAlignment(Qt::AlignVCenter | Qt::AlignRight); 231 | ptc_hlayout->addWidget(iter_no_label); 232 | ptc_hlayout->addWidget(ptc_iteration_number_spin_box_); 233 | 234 | QGroupBox* ptc_gp_box = new QGroupBox(QString("Termination Condition")); 235 | ptc_gp_box->setLayout(ptc_hlayout); 236 | 237 | ptc_iteration_number_spin_box_->setEnabled(false); 238 | ///////////////////////////////////////// 239 | 240 | ///////////////////////////////////////// 241 | // Horizontal Layout for reset and plan 242 | ///////////////////////////////////////// 243 | btn_reset_ = new QPushButton(this); 244 | btn_reset_->setText("Reset"); 245 | connect(btn_reset_, SIGNAL(clicked()), this, SLOT(reset())); 246 | 247 | btn_plan_ = new QPushButton(this); 248 | btn_plan_->setText("Plan"); 249 | connect(btn_plan_, SIGNAL(clicked()), this, SLOT(plan())); 250 | 251 | QHBoxLayout* hlayout = new QHBoxLayout; 252 | hlayout->addWidget(btn_reset_); 253 | hlayout->addWidget(btn_plan_); 254 | ///////////////////////////////////////// 255 | 256 | // Final vertical layout 257 | QVBoxLayout* layout = new QVBoxLayout; 258 | layout->addWidget(query_gp_box); 259 | layout->addWidget(planner_gp_box); 260 | layout->addWidget(planning_obj_gp_box); 261 | layout->addWidget(animation_gp_box); 262 | layout->addWidget(ptc_gp_box); 263 | layout->addLayout(hlayout); 264 | 265 | setLayout(layout); 266 | startCheckBoxStateChanged(true); 267 | ////////////////////////////////////////////// 268 | 269 | start_state_exists_ = false; 270 | goal_state_exists_ = false; 271 | 272 | loadPlannerParameters(); 273 | 274 | btn_reset_->setEnabled(false); 275 | btn_plan_->setEnabled(true); 276 | /////////////////////////////////////////////////////////////// 277 | 278 | ROS_INFO_STREAM_NAMED("Control Panel", "Waiting for service servers to be connected..."); 279 | plan_request_client_.waitForExistence(); 280 | reset_request_client_.waitForExistence(); 281 | start_state_setter_client_.waitForExistence(); 282 | goal_state_setter_client_.waitForExistence(); 283 | ROS_INFO_STREAM_NAMED("Control Panel", "Service servers found."); 284 | 285 | // ROS related 286 | plan_request_client_ = nh_.serviceClient("plan_request", true); 287 | 288 | reset_request_client_ = nh_.serviceClient("reset_request", true); 289 | 290 | start_state_setter_client_ = nh_.serviceClient("set_start_state", true); 291 | 292 | goal_state_setter_client_ = nh_.serviceClient("set_goal_state", true); 293 | 294 | map_bounds_client_ = nh_.serviceClient("get_map_bounds", true); 295 | 296 | // get min and max bounds of the map 297 | ompl_2d_rviz_visualizer_msgs::MapBounds map_bounds_msg; 298 | if (map_bounds_client_.call(map_bounds_msg)) 299 | { 300 | min_bound_x_ = map_bounds_msg.response.min_x; 301 | min_bound_y_ = map_bounds_msg.response.min_y; 302 | max_bound_x_ = map_bounds_msg.response.max_x; 303 | max_bound_y_ = map_bounds_msg.response.max_y; 304 | } 305 | else 306 | { 307 | ROS_ERROR("Failed to call map_bounds_client service."); 308 | exit(1); 309 | } 310 | 311 | start_x_spin_box_->setRange(min_bound_x_, max_bound_x_); 312 | start_y_spin_box_->setRange(min_bound_y_, max_bound_y_); 313 | goal_x_spin_box_->setRange(min_bound_x_, max_bound_x_); 314 | goal_y_spin_box_->setRange(min_bound_y_, max_bound_y_); 315 | } 316 | 317 | void OMPL_ControlPanel::loadPlannerParameters() 318 | { 319 | std::vector ros_param_names; 320 | nh_.getParamNames(ros_param_names); 321 | for (auto i = 1; i < PLANNERS.length(); ++i) 322 | { 323 | PlannerParameterList planner_param_list; 324 | for (const auto& n : ros_param_names) 325 | { 326 | std::size_t found; 327 | if ((found = n.find(PARAMETERS_NS)) != std::string::npos) 328 | { 329 | if (n.find(PLANNERS[i].toStdString()) == std::string::npos) 330 | continue; 331 | 332 | std::string ompl_param_name; 333 | ompl_param_name = n.substr(found + PARAMETERS_NS.length() + PLANNERS[i].toStdString().length() + 1); 334 | 335 | // get the planner parameter 336 | XmlRpc::XmlRpcValue xml_param; 337 | nh_.getParam(n, xml_param); 338 | 339 | // get the planner parameter range 340 | std::string parameter_range_ros_param = PARAMETERS_RANGE_NS + PLANNERS[i].toStdString() + "/" + ompl_param_name; 341 | std::string planner_param_range; 342 | nh_.getParam(parameter_range_ros_param, planner_param_range); 343 | 344 | // set planner parameter 345 | PlannerParameter planner_param; 346 | setPlannerParameter(planner_param, ompl_param_name, xml_param, planner_param_range); 347 | 348 | planner_param_list.push_back(planner_param); 349 | } 350 | } 351 | planners_param_list_.push_back(planner_param_list); 352 | } 353 | } 354 | 355 | void OMPL_ControlPanel::setPlannerParameter(PlannerParameter& param, const std::string& name, 356 | const XmlRpc::XmlRpcValue& value, const std::string& range) 357 | { 358 | param.name = name; 359 | if (value.getType() == XmlRpc::XmlRpcValue::TypeDouble) 360 | { 361 | param.value = static_cast(value); 362 | } 363 | else if (value.getType() == XmlRpc::XmlRpcValue::TypeInt) 364 | { 365 | param.value = static_cast(value); 366 | } 367 | else if (value.getType() == XmlRpc::XmlRpcValue::TypeBoolean) 368 | { 369 | param.value = static_cast(value); 370 | } 371 | param.range = range; 372 | } 373 | 374 | void OMPL_ControlPanel::startCheckBoxStateChanged(bool checked) 375 | { 376 | if (checked) 377 | { 378 | start_combo_box_->setEnabled(true); 379 | btn_start_->setEnabled(true); 380 | 381 | // make sure spinbox is enabled 382 | if (start_combo_box_->currentIndex() == Manual) 383 | { 384 | start_x_spin_box_->setEnabled(true); 385 | start_y_spin_box_->setEnabled(true); 386 | } 387 | } 388 | else 389 | { 390 | start_combo_box_->setEnabled(false); 391 | start_x_spin_box_->setEnabled(false); 392 | start_y_spin_box_->setEnabled(false); 393 | btn_start_->setEnabled(false); 394 | } 395 | } 396 | 397 | void OMPL_ControlPanel::goalCheckBoxStateChanged(bool checked) 398 | { 399 | if (checked) 400 | { 401 | goal_combo_box_->setEnabled(true); 402 | btn_goal_->setEnabled(true); 403 | 404 | // make sure spinbox is enabled 405 | if (goal_combo_box_->currentIndex() == Manual) 406 | { 407 | goal_x_spin_box_->setEnabled(true); 408 | goal_y_spin_box_->setEnabled(true); 409 | } 410 | } 411 | else 412 | { 413 | goal_combo_box_->setEnabled(false); 414 | goal_x_spin_box_->setEnabled(false); 415 | goal_y_spin_box_->setEnabled(false); 416 | btn_goal_->setEnabled(false); 417 | } 418 | } 419 | 420 | void OMPL_ControlPanel::startComboBoxActivated(int index) 421 | { 422 | if (index == Random) 423 | { 424 | start_x_spin_box_->setEnabled(false); 425 | start_y_spin_box_->setEnabled(false); 426 | } 427 | else if (index == Manual) 428 | { 429 | start_x_spin_box_->setEnabled(true); 430 | start_y_spin_box_->setEnabled(true); 431 | } 432 | else if (index == Clicked) 433 | { 434 | start_x_spin_box_->setEnabled(false); 435 | start_y_spin_box_->setEnabled(false); 436 | } 437 | } 438 | 439 | void OMPL_ControlPanel::goalComboBoxActivated(int index) 440 | { 441 | if (index == Random) 442 | { 443 | goal_x_spin_box_->setEnabled(false); 444 | goal_y_spin_box_->setEnabled(false); 445 | } 446 | else if (index == Manual) 447 | { 448 | goal_x_spin_box_->setEnabled(true); 449 | goal_y_spin_box_->setEnabled(true); 450 | } 451 | else if (index == Clicked) 452 | { 453 | goal_x_spin_box_->setEnabled(false); 454 | goal_y_spin_box_->setEnabled(false); 455 | } 456 | } 457 | 458 | void OMPL_ControlPanel::plannerComboBoxActivated(int index) 459 | { 460 | // update planner id 461 | planner_id_ = index; 462 | // delete the parameters 463 | for (const auto layout : planner_params_layout_list_) 464 | { 465 | QLayoutItem* item; 466 | while ((item = layout->takeAt(0)) != nullptr) 467 | { 468 | delete item->widget(); 469 | delete item; 470 | } 471 | delete layout; 472 | } 473 | planner_params_layout_list_.clear(); 474 | updatePlannerParamsLayoutList(static_cast(index)); 475 | } 476 | 477 | void OMPL_ControlPanel::planningObjectiveComboBoxActivated(int index) 478 | { 479 | planning_obj_id_ = index; 480 | } 481 | 482 | void OMPL_ControlPanel::animationModeCheckBoxStateChanged(int state) 483 | { 484 | // animate every and speed 485 | animate_every_spin_box_->setEnabled(animation_mode_check_box_->isChecked()); 486 | animation_speed_slider_->setEnabled(animation_mode_check_box_->isChecked()); 487 | 488 | QStandardItemModel* model = qobject_cast(ptc_combo_box_->model()); 489 | QStandardItem* item = model->item(0); 490 | 491 | if (state == Qt::Checked) 492 | { 493 | item->setEnabled(false); 494 | ptc_combo_box_->setCurrentIndex(1); 495 | planning_duration_spin_box_->setEnabled(false); 496 | ptc_iteration_number_spin_box_->setEnabled(true); 497 | } 498 | else 499 | { 500 | item->setEnabled(true); 501 | } 502 | } 503 | 504 | void OMPL_ControlPanel::ptcComboBoxActivated(int index) 505 | { 506 | if (index == 0) 507 | { 508 | planning_duration_spin_box_->setEnabled(true); 509 | ptc_iteration_number_spin_box_->setEnabled(false); 510 | } 511 | else if (index == 1) 512 | { 513 | planning_duration_spin_box_->setEnabled(false); 514 | ptc_iteration_number_spin_box_->setEnabled(true); 515 | } 516 | } 517 | 518 | bool OMPL_ControlPanel::updatePlannerParamsLayoutList(unsigned int id) 519 | { 520 | if (id == PLANNERS_IDS::INVALID) 521 | return false; 522 | 523 | for (const auto& param : planners_param_list_[id - 1]) 524 | { 525 | QHBoxLayout* params_hlayout = new QHBoxLayout; 526 | QLabel* label = new QLabel(QString::fromStdString(param.name)); 527 | params_hlayout->addWidget(label); 528 | 529 | // double spin box for doubles 530 | if (const double* pval = std::get_if(¶m.value)) 531 | { 532 | QDoubleSpinBox* param_widget = new QDoubleSpinBox; 533 | double min, step, max; 534 | get_range(min, step, max, param.range); 535 | param_widget->setRange(min, max); 536 | param_widget->setValue(*pval); 537 | param_widget->setSingleStep(step); 538 | params_hlayout->addWidget(param_widget); 539 | } 540 | // spin box for ints 541 | else if (const int* pval = std::get_if(¶m.value)) 542 | { 543 | QSpinBox* param_widget = new QSpinBox; 544 | int min, step, max; 545 | get_range(min, step, max, param.range); 546 | param_widget->setRange(min, max); 547 | param_widget->setStepType(QAbstractSpinBox::DefaultStepType); 548 | param_widget->setValue(*pval); 549 | param_widget->setSingleStep(step); 550 | params_hlayout->addWidget(param_widget); 551 | } 552 | // combo box for booleans 553 | else if (const bool* pval = std::get_if(¶m.value)) 554 | { 555 | QComboBox* param_widget = new QComboBox; 556 | param_widget->addItems(COMBO_BOX_BOOLEAN_LIST); 557 | param_widget->setCurrentIndex(*pval ? 1 : 0); 558 | params_hlayout->addWidget(param_widget); 559 | } 560 | 561 | planner_params_layout_list_.append(params_hlayout); 562 | } 563 | 564 | for (const auto layout : planner_params_layout_list_) 565 | { 566 | planner_params_v_layout_->addLayout(layout); 567 | } 568 | return true; 569 | } 570 | 571 | void OMPL_ControlPanel::enablePlannerParamsLayoutList(bool i) 572 | { 573 | for (const auto layout : planner_params_layout_list_) 574 | { 575 | layout->itemAt(1)->widget()->setEnabled(i); 576 | } 577 | } 578 | 579 | void OMPL_ControlPanel::generateRandomPoint(double& x, double& y) 580 | { 581 | std::random_device rd; 582 | rn_gen_ = std::mt19937(rd()); 583 | std::uniform_real_distribution dis_x(min_bound_x_, max_bound_x_); 584 | std::uniform_real_distribution dis_y(min_bound_y_, max_bound_y_); 585 | x = dis_x(rn_gen_); 586 | y = dis_y(rn_gen_); 587 | } 588 | 589 | void OMPL_ControlPanel::btn_start_clicked() 590 | { 591 | // the point needs to be collision-free 592 | ompl_2d_rviz_visualizer_msgs::State msg; 593 | if (start_combo_box_->currentIndex() == Random) 594 | { 595 | generateRandomPoint(msg.request.x, msg.request.y); 596 | start_x_spin_box_->setValue(msg.request.x); 597 | start_y_spin_box_->setValue(msg.request.y); 598 | } 599 | else if (start_combo_box_->currentIndex() == Manual) 600 | { 601 | msg.request.x = start_x_spin_box_->value(); 602 | msg.request.y = start_y_spin_box_->value(); 603 | } 604 | else if (start_combo_box_->currentIndex() == Clicked) 605 | { 606 | // 607 | } 608 | 609 | if (start_state_setter_client_.call(msg)) 610 | { 611 | if (msg.response.success) 612 | { 613 | ROS_DEBUG("set_start_state service calling succeeded."); 614 | start_state_exists_ = true; 615 | } 616 | } 617 | else 618 | { 619 | ROS_ERROR("Failed to call set_start_state service."); 620 | exit(1); 621 | } 622 | } 623 | 624 | void OMPL_ControlPanel::btn_goal_clicked() 625 | { 626 | ompl_2d_rviz_visualizer_msgs::State msg; 627 | if (goal_combo_box_->currentIndex() == Random) 628 | { 629 | generateRandomPoint(msg.request.x, msg.request.y); 630 | goal_x_spin_box_->setValue(msg.request.x); 631 | goal_y_spin_box_->setValue(msg.request.y); 632 | } 633 | else if (goal_combo_box_->currentIndex() == Manual) 634 | { 635 | msg.request.x = goal_x_spin_box_->value(); 636 | msg.request.y = goal_y_spin_box_->value(); 637 | } 638 | else if (goal_combo_box_->currentIndex() == Clicked) 639 | { 640 | // 641 | } 642 | if (goal_state_setter_client_.call(msg)) 643 | { 644 | if (msg.response.success) 645 | { 646 | ROS_DEBUG("set_goal_state service calling succeeded."); 647 | goal_state_exists_ = true; 648 | } 649 | } 650 | else 651 | { 652 | ROS_ERROR("Failed to call set_goal_state service."); 653 | exit(1); 654 | } 655 | } 656 | 657 | void OMPL_ControlPanel::reset() 658 | { 659 | ROS_INFO_STREAM_NAMED("ompl_control_panel", "RESET button pressed."); 660 | 661 | ompl_2d_rviz_visualizer_msgs::Reset msg; 662 | msg.request.clear_graph = true; 663 | if (reset_request_client_.call(msg)) 664 | { 665 | if (msg.response.success) 666 | ROS_DEBUG("Reset Service calling succeeded."); 667 | } 668 | else 669 | { 670 | ROS_ERROR("Failed to call reset_request service."); 671 | exit(1); 672 | } 673 | btn_reset_->setEnabled(false); 674 | 675 | btn_plan_->setEnabled(true); 676 | start_check_box_->setEnabled(true); 677 | goal_check_box_->setEnabled(true); 678 | start_check_box_->setChecked(true); 679 | startCheckBoxStateChanged(true); 680 | planner_combo_box_->setEnabled(true); 681 | enablePlannerParamsLayoutList(true); 682 | planning_objective_combo_box_->setEnabled(true); 683 | animation_mode_check_box_->setEnabled(true); 684 | animation_mode_check_box_->setChecked(false); 685 | ptc_combo_box_->setEnabled(true); 686 | animationModeCheckBoxStateChanged(Qt::Unchecked); 687 | ptcComboBoxActivated(ptc_combo_box_->currentIndex()); 688 | } 689 | 690 | void OMPL_ControlPanel::plan() 691 | { 692 | ROS_INFO_STREAM_NAMED("ompl_control_panel", "PLAN button pressed."); 693 | 694 | if (start_state_exists_ && goal_state_exists_ && planner_id_ != PLANNERS_IDS::INVALID) 695 | { 696 | // read the parameters and update the parameter server 697 | for (const auto layout : planner_params_layout_list_) 698 | { 699 | auto param_name = static_cast(layout->itemAt(0)->widget())->text().toStdString(); 700 | param_name = "ompl_planner_parameters/" + PLANNERS[planner_id_].toStdString() + "/" + param_name; 701 | // get the widget type 702 | std::string widget_type(layout->itemAt(1)->widget()->metaObject()->className()); 703 | if (widget_type == "QDoubleSpinBox") 704 | { 705 | const auto param_val = static_cast(layout->itemAt(1)->widget())->value(); 706 | nh_.setParam(param_name, param_val); 707 | } 708 | else if (widget_type == "QSpinBox") 709 | { 710 | const auto param_val = static_cast(layout->itemAt(1)->widget())->value(); 711 | nh_.setParam(param_name, param_val); 712 | } 713 | else if (widget_type == "QComboBox") 714 | { 715 | const auto idx = static_cast(layout->itemAt(1)->widget())->currentIndex(); 716 | if (COMBO_BOX_BOOLEAN_LIST[idx] == "false") 717 | nh_.setParam(param_name, false); 718 | else 719 | nh_.setParam(param_name, true); 720 | } 721 | } 722 | 723 | if (animation_mode_check_box_->isChecked()) 724 | planning_mode_ = PLANNING_MODE::ANIMATION; 725 | else 726 | { 727 | if (ptc_combo_box_->currentIndex() == 0) 728 | planning_mode_ = PLANNING_MODE::DURATION; 729 | else 730 | planning_mode_ = PLANNING_MODE::ITERATIONS; 731 | } 732 | 733 | ompl_2d_rviz_visualizer_msgs::Plan msg; 734 | msg.request.mode = planning_mode_; 735 | msg.request.animate_every = animate_every_spin_box_->value(); 736 | msg.request.animation_speed = animation_speed_slider_->value() / 100.0; 737 | msg.request.planner_id = planner_id_; 738 | msg.request.objective_id = planning_obj_id_; 739 | msg.request.duration = planning_duration_spin_box_->value(); 740 | msg.request.iterations = ptc_iteration_number_spin_box_->value(); 741 | if (plan_request_client_.call(msg)) 742 | { 743 | if (msg.response.success) 744 | ROS_DEBUG("Plan Service calling succeeded."); 745 | } 746 | else 747 | { 748 | ROS_ERROR("Failed to call plan_request service."); 749 | exit(1); 750 | } 751 | 752 | // enable stuffs 753 | btn_reset_->setEnabled(true); 754 | 755 | // disable stuffs 756 | btn_plan_->setEnabled(false); 757 | start_check_box_->setEnabled(false); 758 | start_combo_box_->setEnabled(false); 759 | start_x_spin_box_->setEnabled(false); 760 | start_y_spin_box_->setEnabled(false); 761 | btn_start_->setEnabled(false); 762 | goal_check_box_->setEnabled(false); 763 | goal_combo_box_->setEnabled(false); 764 | goal_x_spin_box_->setEnabled(false); 765 | goal_y_spin_box_->setEnabled(false); 766 | btn_goal_->setEnabled(false); 767 | planner_combo_box_->setEnabled(false); 768 | enablePlannerParamsLayoutList(false); 769 | planning_objective_combo_box_->setEnabled(false); 770 | animation_mode_check_box_->setEnabled(false); 771 | animate_every_spin_box_->setEnabled(false); 772 | animation_speed_slider_->setEnabled(false); 773 | ptc_combo_box_->setEnabled(false); 774 | planning_duration_spin_box_->setEnabled(false); 775 | ptc_iteration_number_spin_box_->setEnabled(false); 776 | } 777 | ROS_WARN( 778 | "Cannot start planning. Make sure start, goal states and planning " 779 | "algorithm has been selected."); 780 | } 781 | 782 | void OMPL_ControlPanel::save(rviz::Config config) const 783 | { 784 | rviz::Panel::save(config); 785 | } 786 | 787 | void OMPL_ControlPanel::load(const rviz::Config& config) 788 | { 789 | rviz::Panel::load(config); 790 | } 791 | 792 | } // namespace ompl_2d_rviz_visualizer_ros 793 | 794 | #include 795 | PLUGINLIB_EXPORT_CLASS(ompl_2d_rviz_visualizer_ros::OMPL_ControlPanel, rviz::Panel) -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/src/rviz_control_panel.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | BSD 2-Clause License 3 | 4 | Copyright (c) 2021, Phone Thiha Kyaw 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 are met: 9 | 10 | 1. Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | 2. Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 21 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 23 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 24 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 25 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | ******************************************************************************/ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | 57 | namespace ompl_2d_rviz_visualizer_ros 58 | { 59 | static const std::string PARAMETERS_NS = "ompl_planner_parameters/"; 60 | 61 | static const std::string PARAMETERS_RANGE_NS = "ompl_planner_parameters_range/"; 62 | 63 | static const QStringList START_GOAL_COMBO_BOX_ITEMS = { "Generate Random Point", "Type Manually", 64 | "Rviz Clicked Point Tool (WIP)" }; 65 | 66 | static const QStringList PLANNERS = { "", "rrt_connect", "rrt_star" }; 67 | 68 | static const QStringList PTCS = { "Duration in seconds", "Iteration number" }; 69 | 70 | static const QStringList PLANNING_OBJS = { "Minimum path length", "Maximize minimum clearance" }; 71 | 72 | static const QStringList COMBO_BOX_BOOLEAN_LIST = { "false", "true" }; 73 | 74 | enum START_GOAL_COMBO_BOX_IDS 75 | { 76 | Random, 77 | Manual, 78 | Clicked 79 | }; 80 | 81 | enum PLANNERS_IDS 82 | { 83 | INVALID, 84 | RRT_CONNECT, 85 | RRT_STAR 86 | }; 87 | 88 | enum PLANNING_OBJS_IDS 89 | { 90 | PATH_LENGTH, 91 | MAXMIN_CLEARANCE 92 | }; 93 | 94 | enum PLANNING_MODE 95 | { 96 | DURATION, 97 | ITERATIONS, 98 | ANIMATION 99 | }; 100 | 101 | struct PlannerParameter 102 | { 103 | std::string name; 104 | std::variant value; 105 | std::string range; 106 | }; 107 | 108 | class OMPL_ControlPanel : public rviz::Panel 109 | { 110 | Q_OBJECT 111 | public: 112 | OMPL_ControlPanel(QWidget* parent = 0); 113 | 114 | virtual void load(const rviz::Config& config); 115 | virtual void save(rviz::Config config) const; 116 | 117 | public Q_SLOTS: 118 | 119 | protected Q_SLOTS: 120 | void startCheckBoxStateChanged(bool checked); 121 | void goalCheckBoxStateChanged(bool checked); 122 | void startComboBoxActivated(int index); 123 | void goalComboBoxActivated(int index); 124 | void plannerComboBoxActivated(int index); 125 | void planningObjectiveComboBoxActivated(int index); 126 | void animationModeCheckBoxStateChanged(int state); 127 | void ptcComboBoxActivated(int index); 128 | void btn_start_clicked(); 129 | void btn_goal_clicked(); 130 | void reset(); 131 | void plan(); 132 | 133 | protected: 134 | using PlannerParameterList = std::vector; 135 | 136 | void loadPlannerParameters(); 137 | void setPlannerParameter(PlannerParameter& param, const std::string& name, const XmlRpc::XmlRpcValue& value, 138 | const std::string& range); 139 | bool updatePlannerParamsLayoutList(unsigned int id); 140 | void enablePlannerParamsLayoutList(bool i); 141 | 142 | template 143 | void get_range(T& min, T& step, T& max, const std::string& range) 144 | { 145 | std::istringstream ss(range); 146 | std::string s_min, s_step, s_max; 147 | std::getline(ss, s_min, ':'); 148 | std::getline(ss, s_step, ':'); 149 | std::getline(ss, s_max); 150 | if constexpr (std::is_same_v) 151 | { 152 | min = std::stoi(s_min); 153 | step = std::stoi(s_step); 154 | max = std::stoi(s_max); 155 | } 156 | else if constexpr (std::is_same_v) 157 | { 158 | min = std::stod(s_min); 159 | step = std::stod(s_step); 160 | max = std::stod(s_max); 161 | } 162 | }; 163 | 164 | void generateRandomPoint(double& x, double& y); 165 | 166 | // standard Qt objects 167 | QRadioButton* start_check_box_; 168 | QRadioButton* goal_check_box_; 169 | QComboBox* start_combo_box_; 170 | QComboBox* goal_combo_box_; 171 | QDoubleSpinBox* start_x_spin_box_; 172 | QDoubleSpinBox* start_y_spin_box_; 173 | QDoubleSpinBox* goal_x_spin_box_; 174 | QDoubleSpinBox* goal_y_spin_box_; 175 | QPushButton* btn_start_; 176 | QPushButton* btn_goal_; 177 | QComboBox* planner_combo_box_; 178 | QComboBox* planner_params_combo_box_; 179 | QList planner_params_layout_list_; 180 | QVBoxLayout* planner_params_v_layout_; 181 | QPushButton* btn_reset_; 182 | QPushButton* btn_plan_; 183 | QComboBox* planning_objective_combo_box_; 184 | QDoubleSpinBox* planning_duration_spin_box_; 185 | QSpinBox* ptc_iteration_number_spin_box_; 186 | QCheckBox* animation_mode_check_box_; 187 | QSpinBox* animate_every_spin_box_; 188 | QSlider* animation_speed_slider_; 189 | QComboBox* ptc_combo_box_; 190 | 191 | // container to store planner parameters 192 | std::vector planners_param_list_; 193 | 194 | // planning request stuffs 195 | int planner_id_; 196 | int planning_obj_id_; 197 | int planning_mode_; 198 | 199 | // start and goal flags 200 | bool start_state_exists_; 201 | bool goal_state_exists_; 202 | 203 | // map bounds 204 | double min_bound_x_; 205 | double max_bound_x_; 206 | double min_bound_y_; 207 | double max_bound_y_; 208 | 209 | // random number generator 210 | std::mt19937 rn_gen_; 211 | 212 | // ROS related 213 | ros::NodeHandle nh_; 214 | ros::NodeHandle prv_nh_; 215 | ros::ServiceClient plan_request_client_; 216 | ros::ServiceClient reset_request_client_; 217 | ros::ServiceClient start_state_setter_client_; 218 | ros::ServiceClient goal_state_setter_client_; 219 | ros::ServiceClient map_bounds_client_; 220 | }; 221 | 222 | } // namespace ompl_2d_rviz_visualizer_ros 223 | -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/src/rviz_renderer.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | BSD 2-Clause License 3 | 4 | Copyright (c) 2021, Phone Thiha Kyaw 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 are met: 9 | 10 | 1. Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | 2. Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 21 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 23 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 24 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 25 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | ******************************************************************************/ 28 | 29 | #include 30 | 31 | namespace ompl_2d_rviz_visualizer_ros 32 | { 33 | RvizRenderer::RvizRenderer(const std::string& base_frame, const std::string& marker_topic, ros::NodeHandle nh) 34 | : base_frame_{ base_frame } 35 | { 36 | visual_tools_ = std::make_shared(base_frame_, marker_topic, nh); 37 | visual_tools_->loadMarkerPub(); 38 | visual_tools_->deleteAllMarkers(); 39 | visual_tools_->enableBatchPublishing(); 40 | } 41 | 42 | RvizRenderer::~RvizRenderer() 43 | { 44 | } 45 | 46 | bool RvizRenderer::deleteAllMarkers() 47 | { 48 | visual_tools_->deleteAllMarkers(); 49 | return visual_tools_->trigger(); 50 | } 51 | 52 | void RvizRenderer::addToMarkerIDs(const std::string& ns, std::size_t id) 53 | { 54 | auto marker_ids_it = marker_ids_.find(ns); 55 | if (marker_ids_it != marker_ids_.end()) 56 | { 57 | marker_ids_it->second.insert(id); 58 | } 59 | else 60 | { 61 | marker_ids_[ns] = std::unordered_set{ id }; 62 | } 63 | } 64 | 65 | bool RvizRenderer::deleteAllMarkersInNS(const std::string& ns) 66 | { 67 | // check specific namespace has already been published 68 | auto marker_ids_it = marker_ids_.find(ns); 69 | if (marker_ids_it != marker_ids_.end()) 70 | { 71 | // if so then we create temporary marker and publish it with delete action 72 | visualization_msgs::Marker temp_marker; 73 | temp_marker.header.frame_id = base_frame_; 74 | temp_marker.ns = ns; 75 | temp_marker.action = visualization_msgs::Marker::DELETE; 76 | for (const auto id : marker_ids_it->second) 77 | { 78 | temp_marker.id = id; 79 | temp_marker.header.stamp = ros::Time::now(); 80 | visual_tools_->publishMarker(temp_marker); 81 | } 82 | 83 | // don't forget to remove the ns from marker_ids 84 | marker_ids_.erase(ns); 85 | return visual_tools_->trigger(); 86 | } 87 | else 88 | { 89 | ROS_DEBUG("Namespace %s not found in marker IDs.", ns.c_str()); 90 | return false; 91 | } 92 | } 93 | 94 | bool RvizRenderer::deleteMarkerInNSAndID(const std::string& ns, std::size_t id) 95 | { 96 | // check specific namespace has already been published 97 | auto marker_ids_ns_it = marker_ids_.find(ns); 98 | if (marker_ids_ns_it != marker_ids_.end()) 99 | { 100 | // if so, we find the specific id 101 | auto marker_ids_id_it = marker_ids_ns_it->second.find(id); 102 | if (marker_ids_id_it != marker_ids_ns_it->second.end()) 103 | { 104 | // if the id is found, 105 | // publish temporary marker with delete action 106 | visualization_msgs::Marker temp_marker; 107 | temp_marker.header.frame_id = base_frame_; 108 | temp_marker.ns = ns; 109 | temp_marker.action = visualization_msgs::Marker::DELETE; 110 | temp_marker.id = id; 111 | temp_marker.header.stamp = ros::Time::now(); 112 | visual_tools_->publishMarker(temp_marker); 113 | 114 | // don't forget to remove the id from marker_ids ns 115 | marker_ids_ns_it->second.erase(id); 116 | return visual_tools_->trigger(); 117 | } 118 | else 119 | { 120 | ROS_DEBUG("ID %ld not found in marker namespace %s.", id, ns.c_str()); 121 | return false; 122 | } 123 | } 124 | else 125 | { 126 | ROS_DEBUG("Namespace %s not found in marker IDs.", ns.c_str()); 127 | return false; 128 | } 129 | } 130 | 131 | bool RvizRenderer::renderState(const ob::State* state, const rvt::colors& color, const rvt::scales& scale, 132 | const std::string& ns, std::size_t id) 133 | { 134 | visual_tools_->publishSphere(stateToPoint(state), color, scale, ns, id); 135 | 136 | // remember this marker 137 | addToMarkerIDs(ns, id); 138 | 139 | return visual_tools_->trigger(); 140 | } 141 | 142 | bool RvizRenderer::renderState(const Eigen::Vector3d& point, const rvt::colors& color, const rvt::scales& scale, 143 | const std::string& ns, std::size_t id) 144 | { 145 | visual_tools_->publishSphere(point, color, scale, ns, id); 146 | 147 | // remember this marker 148 | addToMarkerIDs(ns, id); 149 | 150 | return visual_tools_->trigger(); 151 | } 152 | 153 | bool RvizRenderer::renderPath(const og::PathGeometric& path, const rvt::colors& color, const double radius, 154 | const std::string& ns) 155 | { 156 | if (path.getStateCount() <= 0) 157 | { 158 | ROS_WARN("No states found in path"); 159 | return false; 160 | } 161 | 162 | // Initialize first vertex 163 | Eigen::Vector3d prev_vertex = stateToPoint(path.getState(0)); 164 | Eigen::Vector3d this_vertex; 165 | 166 | std::size_t id = 1; 167 | 168 | // Convert path coordinates 169 | for (std::size_t i = 1; i < path.getStateCount(); ++i) 170 | { 171 | // Get current coordinates 172 | this_vertex = stateToPoint(path.getState(i)); 173 | 174 | // Create line 175 | publishCylinder(prev_vertex, this_vertex, color, radius, ns, id); 176 | addToMarkerIDs(ns, id); 177 | id++; 178 | 179 | // Save these coordinates for next line 180 | prev_vertex = this_vertex; 181 | } 182 | return visual_tools_->trigger(); 183 | } 184 | 185 | bool RvizRenderer::renderGraph(const ob::PlannerDataPtr planner_data, const rvt::colors& color, const double radius, 186 | const std::string& ns) 187 | { 188 | graph_msgs::GeometryGraph graph; 189 | 190 | for (std::size_t vertex_id = 0; vertex_id < planner_data->numVertices(); ++vertex_id) 191 | { 192 | ob::PlannerDataVertex* vertex = &planner_data->getVertex(vertex_id); 193 | auto this_vertex = stateToPointMsg(vertex->getState()); 194 | graph.nodes.push_back(this_vertex); 195 | 196 | graph_msgs::Edges edge_msg; 197 | // Get the out edges from the current vertex 198 | std::vector edge_list; 199 | planner_data->getEdges(vertex_id, edge_msg.node_ids); 200 | graph.edges.push_back(edge_msg); 201 | } 202 | 203 | // Track which pairs of nodes we've already connected since graph is 204 | // bi-directional 205 | std::size_t id = 0; 206 | typedef std::pair node_ids; 207 | std::set added_edges; 208 | std::pair::iterator, bool> return_value; 209 | Eigen::Vector3d a, b; 210 | for (std::size_t i = 0; i < graph.nodes.size(); ++i) 211 | { 212 | for (std::size_t j = 0; j < graph.edges[i].node_ids.size(); ++j) 213 | { 214 | // Check if we've already added this pair of nodes (edge) 215 | return_value = added_edges.insert(node_ids(i, j)); 216 | if (!return_value.second) 217 | { 218 | // Element already existed in set, so don't add a new collision object 219 | } 220 | else 221 | { 222 | // Create a cylinder from two points 223 | a = visual_tools_->convertPoint(graph.nodes[i]); 224 | b = visual_tools_->convertPoint(graph.nodes[graph.edges[i].node_ids[j]]); 225 | publishCylinder(a, b, color, radius, ns, id); 226 | addToMarkerIDs(ns, id); 227 | id++; 228 | } 229 | } 230 | } 231 | visual_tools_->trigger(); 232 | 233 | // We are currently overwriting IDs with ADD option (which is still ok) 234 | // But if our graph is pruned, there will be left-over IDs with higher 235 | // numbers, which needs to be deleted (remove from the rviz scene). 236 | // One way to solve this might be - delete unused IDs from this current 237 | // namespace using stored marker_ids_. 238 | 239 | // delete left-over markers 240 | // run until marker id not found in namespace 241 | while (deleteMarkerInNSAndID(ns, id++)) 242 | { 243 | } 244 | 245 | return true; 246 | } 247 | 248 | bool RvizRenderer::publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, 249 | const rvt::colors& color, const double radius, const std::string& ns, std::size_t id) 250 | { 251 | visualization_msgs::Marker cylinder_marker; 252 | cylinder_marker.lifetime = ros::Duration(0.0); 253 | cylinder_marker.header.frame_id = base_frame_; 254 | // Set the marker action. Options are ADD and DELETE 255 | cylinder_marker.action = visualization_msgs::Marker::ADD; 256 | // Set the marker type. 257 | cylinder_marker.type = visualization_msgs::Marker::CYLINDER; 258 | 259 | // Distance between two points 260 | double height = (point1 - point2).lpNorm<2>(); 261 | 262 | // Find center point 263 | Eigen::Vector3d pt_center = visual_tools_->getCenterPoint(point1, point2); 264 | 265 | // Create vector 266 | Eigen::Isometry3d pose; 267 | pose = visual_tools_->getVectorBetweenPoints(pt_center, point2); 268 | 269 | // Convert pose to be normal to cylindar axis 270 | Eigen::Isometry3d rotation; 271 | rotation = Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY()); 272 | pose = pose * rotation; 273 | 274 | // Set the timestamp 275 | cylinder_marker.header.stamp = ros::Time::now(); 276 | cylinder_marker.ns = ns; 277 | cylinder_marker.id = id; 278 | 279 | // Set the pose 280 | cylinder_marker.pose = visual_tools_->convertPose(pose); 281 | 282 | // Set marker size 283 | cylinder_marker.scale.x = radius; 284 | cylinder_marker.scale.y = radius; 285 | cylinder_marker.scale.z = height; 286 | 287 | // Set marker color 288 | cylinder_marker.color = visual_tools_->getColor(color); 289 | 290 | return visual_tools_->publishMarker(cylinder_marker); 291 | } 292 | 293 | Eigen::Vector3d RvizRenderer::stateToPoint(const ob::State* state) 294 | { 295 | if (!state) 296 | { 297 | ROS_FATAL("No state found for vertex"); 298 | exit(1); 299 | } 300 | // Convert to RealVectorStateSpace 301 | const ob::RealVectorStateSpace::StateType* real_state = 302 | static_cast(state); 303 | 304 | // Create point 305 | Eigen::Vector3d temp_eigen_point; 306 | temp_eigen_point.x() = real_state->values[0]; 307 | temp_eigen_point.y() = real_state->values[1]; 308 | temp_eigen_point.z() = 0.0; 309 | 310 | return temp_eigen_point; 311 | } 312 | 313 | geometry_msgs::Point RvizRenderer::stateToPointMsg(const ob::State* state) 314 | { 315 | if (!state) 316 | { 317 | ROS_FATAL("No state found for vertex"); 318 | exit(1); 319 | } 320 | 321 | // Convert to RealVectorStateSpace 322 | const ob::RealVectorStateSpace::StateType* real_state = 323 | static_cast(state); 324 | 325 | // Create point 326 | geometry_msgs::Point temp_point; 327 | temp_point.x = real_state->values[0]; 328 | temp_point.y = real_state->values[1]; 329 | temp_point.z = 0.0; 330 | 331 | return temp_point; 332 | } 333 | 334 | } // namespace ompl_2d_rviz_visualizer_ros -------------------------------------------------------------------------------- /ompl_2d_rviz_visualizer_ros/src/visualizer_nodelet.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | BSD 2-Clause License 3 | 4 | Copyright (c) 2021, Phone Thiha Kyaw 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 are met: 9 | 10 | 1. Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | 2. Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 21 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 23 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 24 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 25 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | ******************************************************************************/ 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #include 55 | #include 56 | #include 57 | #include 58 | 59 | namespace ob = ompl::base; 60 | namespace og = ompl::geometric; 61 | namespace ot = ompl::time; 62 | 63 | namespace ompl_2d_rviz_visualizer_ros 64 | { 65 | enum PLANNERS_IDS 66 | { 67 | INVALID, 68 | RRT_CONNECT, 69 | RRT_STAR 70 | }; 71 | 72 | enum PLANNING_OBJS_IDS 73 | { 74 | PATH_LENGTH, 75 | MAXMIN_CLEARANCE 76 | }; 77 | 78 | enum PLANNING_MODE 79 | { 80 | DURATION, 81 | ITERATIONS, 82 | ANIMATION 83 | }; 84 | 85 | const std::vector PLANNER_NAMES{ "invalid", "rrt_connect", "rrt_star" }; 86 | 87 | class VisualizerNodelet : public nodelet::Nodelet 88 | { 89 | public: 90 | VisualizerNodelet() 91 | { 92 | } 93 | virtual ~VisualizerNodelet() 94 | { 95 | } 96 | 97 | void onInit() override 98 | { 99 | NODELET_DEBUG("initializing ompl_2d_rviz_visualizer nodelet..."); 100 | 101 | // initialize node handlers 102 | nh_ = getNodeHandle(); 103 | mt_nh_ = getMTNodeHandle(); 104 | private_nh_ = getPrivateNodeHandle(); 105 | 106 | // initialize rviz renderer object 107 | rviz_renderer_ = 108 | std::make_shared("map", "/ompl_2d_rviz_visualizer_nodelet/rviz_visual_markers", mt_nh_); 109 | 110 | // ompl related 111 | space_ = std::make_shared(2u); 112 | 113 | // initialize map loader and occupancy grid map objects 114 | ogm_map_ = std::make_shared(); 115 | map_loader_ = std::make_shared("map", "/ompl_2d_rviz_visualizer_nodelet/map", mt_nh_); 116 | 117 | if (!private_nh_.hasParam("map_file_path")) 118 | { 119 | ROS_ERROR("map_file_path does not exist in parameter server. Exiting..."); 120 | exit(1); 121 | } 122 | 123 | std::string map_file_path; 124 | private_nh_.getParam("map_file_path", map_file_path); 125 | map_loader_->loadMapFromYaml(map_file_path, *ogm_map_); 126 | 127 | //////////////////////////////////////////////////////////////// 128 | // set the bounds for the R^2 129 | if (!map_utils::getBounds(map_bounds_.min_x, map_bounds_.max_x, map_bounds_.min_y, map_bounds_.max_y, *ogm_map_)) 130 | { 131 | ROS_ERROR("Fail to generate bounds in the occupancy grid map."); 132 | exit(1); 133 | } 134 | 135 | ob::RealVectorBounds bounds(2); 136 | bounds.setLow(0, map_bounds_.min_x); 137 | bounds.setLow(1, map_bounds_.min_y); 138 | bounds.setHigh(0, map_bounds_.max_x); 139 | bounds.setHigh(1, map_bounds_.max_y); 140 | 141 | space_->as()->setBounds(bounds); 142 | //////////////////////////////////////////////////////////////// 143 | 144 | space_->setup(); 145 | 146 | ss_ = std::make_shared(space_); 147 | si_ = ss_->getSpaceInformation(); 148 | 149 | start_state_ = std::make_shared>(space_); 150 | goal_state_ = std::make_shared>(space_); 151 | 152 | // custom State Validity Checker class need to be implemented which uses 153 | // occupancy grid maps for collison checking 154 | collision_checker_ = std::make_shared(ogm_map_); 155 | 156 | ss_->setStateValidityChecker([this](const ob::State* state) { return collision_checker_->isValid(state); }); 157 | 158 | enable_planning_ = false; 159 | planning_initialized_ = false; 160 | planner_id_ = PLANNERS_IDS::INVALID; 161 | planning_obj_id_ = PLANNING_OBJS_IDS::PATH_LENGTH; 162 | planning_duration_ = 0.0; 163 | animation_speed_ = 1.0; 164 | curr_iter_ = 0u; 165 | animate_every_ = 1u; 166 | planning_iterations_ = 1u; 167 | 168 | // service servers 169 | start_state_setter_srv_ = mt_nh_.advertiseService("/ompl_2d_rviz_visualizer_nodelet/set_start_state", 170 | &VisualizerNodelet::setStartStateService, this); 171 | 172 | goal_state_setter_srv_ = mt_nh_.advertiseService("/ompl_2d_rviz_visualizer_nodelet/set_goal_state", 173 | &VisualizerNodelet::setGoalStateService, this); 174 | 175 | plan_request_srv_ = mt_nh_.advertiseService("/ompl_2d_rviz_visualizer_nodelet/plan_request", 176 | &VisualizerNodelet::planRequestService, this); 177 | 178 | reset_request_srv_ = mt_nh_.advertiseService("/ompl_2d_rviz_visualizer_nodelet/reset_request", 179 | &VisualizerNodelet::resetRequestService, this); 180 | 181 | map_bounds_srv_ = mt_nh_.advertiseService("/ompl_2d_rviz_visualizer_nodelet/get_map_bounds", 182 | &VisualizerNodelet::getMapBoundsService, this); 183 | 184 | // timers 185 | // TODO: this interval must be set from ros param 186 | planning_timer_interval_ = 0.001; 187 | planning_timer_ = 188 | mt_nh_.createWallTimer(ros::WallDuration(planning_timer_interval_), &VisualizerNodelet::planningTimerCB, this); 189 | } 190 | 191 | bool setStartStateService(ompl_2d_rviz_visualizer_msgs::StateRequest& req, 192 | ompl_2d_rviz_visualizer_msgs::StateResponse& res) 193 | { 194 | if (!(collision_checker_->isValid(req.x, req.y))) 195 | { 196 | ROS_INFO("The start state is in collision."); 197 | res.success = false; 198 | return true; 199 | } 200 | 201 | (*start_state_)[0] = req.x; 202 | (*start_state_)[1] = req.y; 203 | rviz_renderer_->renderState((*start_state_).get(), rviz_visual_tools::GREEN, rviz_visual_tools::XLARGE, 204 | "start_goal_states", 1); 205 | res.success = true; 206 | return true; 207 | } 208 | 209 | bool setGoalStateService(ompl_2d_rviz_visualizer_msgs::StateRequest& req, 210 | ompl_2d_rviz_visualizer_msgs::StateResponse& res) 211 | { 212 | if (!(collision_checker_->isValid(req.x, req.y))) 213 | { 214 | ROS_INFO("The goal state is in collision."); 215 | res.success = false; 216 | return true; 217 | } 218 | (*goal_state_)[0] = req.x; 219 | (*goal_state_)[1] = req.y; 220 | rviz_renderer_->renderState((*goal_state_).get(), rviz_visual_tools::RED, rviz_visual_tools::XLARGE, 221 | "start_goal_states", 2); 222 | res.success = true; 223 | return true; 224 | } 225 | 226 | bool planRequestService(ompl_2d_rviz_visualizer_msgs::PlanRequest& req, 227 | ompl_2d_rviz_visualizer_msgs::PlanResponse& res) 228 | { 229 | ROS_INFO("Planning request received."); 230 | ROS_INFO("Planner ID: %s", PLANNER_NAMES[req.planner_id].c_str()); 231 | 232 | planning_mode_ = req.mode; 233 | 234 | // TODO: these two must be thread safe and editable during planning 235 | animate_every_ = req.animate_every; 236 | animation_speed_ = req.animation_speed; 237 | 238 | planner_id_ = req.planner_id; 239 | planning_obj_id_ = req.objective_id; 240 | planning_duration_ = req.duration; 241 | planning_iterations_ = req.iterations; 242 | 243 | enable_planning_ = true; 244 | planning_resetted_ = false; 245 | 246 | res.success = true; 247 | return true; 248 | } 249 | 250 | bool resetRequestService(ompl_2d_rviz_visualizer_msgs::ResetRequest& req, 251 | ompl_2d_rviz_visualizer_msgs::ResetResponse& res) 252 | { 253 | ROS_INFO("Resetting the planner and clearing the graph markers."); 254 | 255 | planning_initialized_ = false; 256 | enable_planning_ = false; 257 | planning_resetted_ = true; 258 | res.success = true; 259 | return true; 260 | } 261 | 262 | bool getMapBoundsService(ompl_2d_rviz_visualizer_msgs::MapBoundsRequest& req, 263 | ompl_2d_rviz_visualizer_msgs::MapBoundsResponse& res) 264 | { 265 | res.min_x = map_bounds_.min_x; 266 | res.min_y = map_bounds_.min_y; 267 | res.max_x = map_bounds_.max_x; 268 | res.max_y = map_bounds_.max_y; 269 | return true; 270 | } 271 | 272 | void initPlanning() 273 | { 274 | // choose the optimization objective 275 | switch (planning_obj_id_) 276 | { 277 | case PATH_LENGTH: 278 | optimization_objective_ = std::make_shared(si_); 279 | break; 280 | 281 | case MAXMIN_CLEARANCE: 282 | optimization_objective_ = std::make_shared(si_); 283 | break; 284 | 285 | default: 286 | ROS_ERROR("Error setting OMPL optimization objective."); 287 | exit(1); 288 | break; 289 | } 290 | ss_->setOptimizationObjective(optimization_objective_); 291 | 292 | // choose the planner 293 | switch (planner_id_) 294 | { 295 | case RRT_CONNECT: 296 | ss_->setPlanner(ob::PlannerPtr(std::make_shared(si_))); 297 | break; 298 | case RRT_STAR: 299 | ss_->setPlanner(ob::PlannerPtr(std::make_shared(si_))); 300 | break; 301 | default: 302 | break; 303 | } 304 | 305 | // get latest planner params from the parameter server 306 | std::vector param_names; 307 | ss_->getPlanner()->params().getParamNames(param_names); 308 | std::map updated_param_names_values; 309 | for (const auto& n : param_names) 310 | { 311 | std::string param_name = "ompl_planner_parameters/" + PLANNER_NAMES[planner_id_] + "/" + n; 312 | XmlRpc::XmlRpcValue param; 313 | if (private_nh_.hasParam(param_name)) 314 | { 315 | private_nh_.getParam(param_name, param); 316 | 317 | if (param.getType() == XmlRpc::XmlRpcValue::TypeDouble) 318 | { 319 | updated_param_names_values[n] = std::to_string(static_cast(param)); 320 | } 321 | else if (param.getType() == XmlRpc::XmlRpcValue::TypeInt) 322 | { 323 | updated_param_names_values[n] = std::to_string(static_cast(param)); 324 | } 325 | else if (param.getType() == XmlRpc::XmlRpcValue::TypeBoolean) 326 | { 327 | updated_param_names_values[n] = std::to_string(static_cast(param)); 328 | } 329 | } 330 | } 331 | ss_->getPlanner()->params().setParams(updated_param_names_values); 332 | 333 | ROS_INFO("Planner parameters updated."); 334 | 335 | ss_->clear(); 336 | ss_->clearStartStates(); 337 | 338 | // set the start and goal states 339 | ss_->setStartAndGoalStates(*start_state_, *goal_state_); 340 | 341 | ss_->setup(); 342 | 343 | curr_iter_ = 0u; 344 | 345 | planning_init_time_ = ot::now(); 346 | planning_initialized_ = true; 347 | } 348 | 349 | void planningTimerCB(const ros::WallTimerEvent& event) 350 | { 351 | if (enable_planning_) 352 | { 353 | if (!planning_initialized_) 354 | initPlanning(); 355 | 356 | // ANIMATION MODE 357 | if (planning_mode_ == PLANNING_MODE::ANIMATION) 358 | { 359 | // we control the planning and animating time 360 | // Note: only control in animating mode 361 | 362 | // calculate the animation time 363 | animation_speed_mutex_.lock(); 364 | auto time_diff = planning_timer_interval_ / animation_speed_; 365 | animation_speed_mutex_.unlock(); 366 | 367 | if ((ot::seconds(ot::now() - planning_init_time_)) < time_diff) 368 | return; 369 | planning_init_time_ = ot::now(); 370 | 371 | // create the planner termination condition 372 | ob::PlannerTerminationCondition ptc = ob::PlannerTerminationCondition(ob::IterationTerminationCondition(1u)); 373 | ob::PlannerStatus status = ss_->getPlanner()->solve(ptc); 374 | // increase iteration number 375 | ++curr_iter_; 376 | 377 | // render planner data in every n iteration 378 | if (curr_iter_ % animate_every_ == 0) 379 | { 380 | // render the graph 381 | const ob::PlannerDataPtr planner_data(std::make_shared(si_)); 382 | ss_->getPlannerData(*planner_data); 383 | rviz_renderer_->renderGraph(planner_data, rviz_visual_tools::BLUE, 0.005, "planner_graph"); 384 | } 385 | if (status) 386 | { 387 | // render path 388 | ss_->getSolutionPath().interpolate(); 389 | rviz_renderer_->deleteAllMarkersInNS("final_solution"); 390 | rviz_renderer_->renderPath(ss_->getSolutionPath(), rviz_visual_tools::PURPLE, 0.02, "final_solution"); 391 | } 392 | 393 | if (curr_iter_ == planning_iterations_) 394 | { 395 | enable_planning_ = false; 396 | ROS_INFO("Planning finished."); 397 | if (status) 398 | { 399 | // render path 400 | ss_->getSolutionPath().interpolate(); 401 | rviz_renderer_->deleteAllMarkersInNS("final_solution"); 402 | rviz_renderer_->renderPath(ss_->getSolutionPath(), rviz_visual_tools::PURPLE, 0.02, "final_solution"); 403 | } 404 | } 405 | } 406 | // DEFAULT MODE 407 | else if (planning_mode_ == PLANNING_MODE::DURATION || planning_mode_ == PLANNING_MODE::ITERATIONS) 408 | { 409 | // Create the termination condition 410 | std::shared_ptr ptc; 411 | if (planning_mode_ == PLANNING_MODE::DURATION) 412 | ptc = std::make_shared( 413 | ob::timedPlannerTerminationCondition(planning_duration_, 0.01)); 414 | else 415 | ptc = std::make_shared( 416 | ob::PlannerTerminationCondition(ob::IterationTerminationCondition(planning_iterations_))); 417 | 418 | // attempt to solve the problem within x seconds of planning time 419 | ob::PlannerStatus solved; 420 | solved = ss_->solve(*ptc); 421 | 422 | // render graph 423 | const ob::PlannerDataPtr planner_data(std::make_shared(si_)); 424 | ss_->getPlannerData(*planner_data); 425 | 426 | ROS_INFO("Number of start vertices: %d", planner_data->numStartVertices()); 427 | ROS_INFO("Number of goal vertices: %d", planner_data->numGoalVertices()); 428 | ROS_INFO("Number of vertices: %d", planner_data->numVertices()); 429 | ROS_INFO("Number of edges: %d", planner_data->numEdges()); 430 | 431 | rviz_renderer_->renderGraph(planner_data, rviz_visual_tools::BLUE, 0.005, "planner_graph"); 432 | enable_planning_ = false; 433 | if (solved) 434 | { 435 | // render path 436 | ss_->getSolutionPath().interpolate(); 437 | 438 | rviz_renderer_->renderPath(ss_->getSolutionPath(), rviz_visual_tools::PURPLE, 0.02, "final_solution"); 439 | } 440 | } 441 | else 442 | { 443 | // error 444 | } 445 | } 446 | else if (planning_resetted_) 447 | { 448 | // only clear the graph and path markers 449 | rviz_renderer_->deleteAllMarkersInNS("planner_graph"); 450 | rviz_renderer_->deleteAllMarkersInNS("final_solution"); 451 | planning_resetted_ = false; 452 | } 453 | } 454 | 455 | private: 456 | // ROS related 457 | // node handles 458 | ros::NodeHandle nh_; 459 | ros::NodeHandle mt_nh_; 460 | ros::NodeHandle private_nh_; 461 | 462 | // service servers 463 | ros::ServiceServer plan_request_srv_; 464 | ros::ServiceServer reset_request_srv_; 465 | ros::ServiceServer start_state_setter_srv_; 466 | ros::ServiceServer goal_state_setter_srv_; 467 | ros::ServiceServer map_bounds_srv_; 468 | 469 | // timers 470 | double planning_timer_interval_; 471 | ros::WallTimer planning_timer_; 472 | 473 | // rendering stuffs 474 | RvizRendererPtr rviz_renderer_; 475 | 476 | // ogm related 477 | struct MapBounds 478 | { 479 | double min_x; 480 | double min_y; 481 | double max_x; 482 | double max_y; 483 | }; 484 | 485 | MapBounds map_bounds_; 486 | std::shared_ptr ogm_map_; 487 | std::shared_ptr map_loader_; 488 | 489 | // ompl related 490 | ob::StateSpacePtr space_; 491 | og::SimpleSetupPtr ss_; 492 | ob::SpaceInformationPtr si_; 493 | std::shared_ptr optimization_objective_; 494 | ob::ScopedStatePtr start_state_; 495 | ob::ScopedStatePtr goal_state_; 496 | 497 | // collision checker 498 | std::shared_ptr collision_checker_; 499 | 500 | int planning_mode_; 501 | int planner_id_; 502 | int planning_obj_id_; 503 | double planning_duration_; 504 | unsigned int planning_iterations_; 505 | ot::point planning_init_time_; 506 | unsigned int curr_iter_; 507 | 508 | unsigned int animate_every_; 509 | double animation_speed_; 510 | std::mutex animation_speed_mutex_; 511 | 512 | // flags 513 | std::atomic_bool planning_initialized_; 514 | std::atomic_bool planning_resetted_; 515 | std::atomic_bool enable_planning_; 516 | }; 517 | } // namespace ompl_2d_rviz_visualizer_ros 518 | 519 | PLUGINLIB_EXPORT_CLASS(ompl_2d_rviz_visualizer_ros::VisualizerNodelet, nodelet::Nodelet) --------------------------------------------------------------------------------