├── esc_move_base_control ├── resource │ └── esc_move_base_control ├── esc_move_base_control │ ├── __init__.py │ └── base_controller.py ├── setup.cfg ├── config │ └── esc_move_base_control.yaml ├── launch │ └── esc_move_base_control.launch ├── setup.py ├── test │ ├── test_pep257.py │ ├── test_flake8.py │ └── test_copyright.py ├── LICENSE └── package.xml ├── esc_move_base_msgs ├── msg │ └── Path2D.msg ├── action │ ├── Goto2D.action │ └── GotoRegion2D.action ├── CMakeLists.txt └── package.xml ├── esc_move_base_planning ├── launch │ └── esc_move_base_planning.launch ├── config │ └── esc_move_base_planning.yaml ├── src │ ├── state_cost_objective.cpp │ ├── state_validity_checker_grid_map_R2.cpp │ ├── new_state_sampler.cpp │ └── planner │ │ └── RRTstarMod.cpp ├── package.xml ├── CMakeLists.txt └── include │ ├── state_validity_checker_grid_map_R2.h │ ├── state_cost_objective.h │ ├── new_state_sampler.h │ └── planner │ └── RRTstarMod.h ├── esc_move_base_mapping ├── launch │ └── esc_move_base_mapping.launch ├── config │ └── esc_move_base_mapping.yaml ├── package.xml ├── CMakeLists.txt ├── include │ └── world_modeler.hpp └── src │ └── world_modeler.cpp ├── .gitignore ├── LICENSE └── README.md /esc_move_base_control/resource/esc_move_base_control: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /esc_move_base_control/esc_move_base_control/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /esc_move_base_msgs/msg/Path2D.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose2D[] waypoints 2 | -------------------------------------------------------------------------------- /esc_move_base_control/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/esc_move_base_control 3 | [install] 4 | install_scripts=$base/lib/esc_move_base_control 5 | -------------------------------------------------------------------------------- /esc_move_base_msgs/action/Goto2D.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | geometry_msgs/Pose2D goal 3 | --- 4 | #result definition 5 | bool success 6 | --- 7 | #feedback -------------------------------------------------------------------------------- /esc_move_base_msgs/action/GotoRegion2D.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | geometry_msgs/Pose2D goal 3 | float32 radius 4 | --- 5 | #result definition 6 | bool success 7 | --- 8 | #feedback -------------------------------------------------------------------------------- /esc_move_base_control/config/esc_move_base_control.yaml: -------------------------------------------------------------------------------- 1 | # Control parameters 2 | max_vel: 0.38 3 | min_vel: 0.05 4 | max_turn_rate: 0.7 5 | controller_hz: 100.0 6 | control_path_topic: /esc_move_base_planner/esc_move_base_solution_path 7 | control_output_topic: /pepper/cmd_vel 8 | odometry_topic: /pepper/odom_groundtruth 9 | -------------------------------------------------------------------------------- /esc_move_base_planning/launch/esc_move_base_planning.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /esc_move_base_control/launch/esc_move_base_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /esc_move_base_mapping/launch/esc_move_base_mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | .vscode/ 35 | build/ 36 | devel/ 37 | bags/ 38 | .qi/ 39 | __pycache__/ 40 | -------------------------------------------------------------------------------- /esc_move_base_control/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages, setup 2 | 3 | package_name = "esc_move_base_control" 4 | 5 | setup( 6 | name=package_name, 7 | version="0.0.0", 8 | packages=find_packages(exclude=["test"]), 9 | data_files=[ 10 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 11 | ("share/" + package_name, ["package.xml"]), 12 | ], 13 | install_requires=["setuptools"], 14 | zip_safe=True, 15 | maintainer="sasm", 16 | maintainer_email="sasilva1998@gmail.com", 17 | description="TODO: Package description", 18 | license="MIT", 19 | tests_require=["pytest"], 20 | entry_points={ 21 | "console_scripts": [ 22 | "base_controller = esc_move_base_control.base_controller:main" 23 | ], 24 | }, 25 | ) 26 | -------------------------------------------------------------------------------- /esc_move_base_control/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /esc_move_base_mapping/config/esc_move_base_mapping.yaml: -------------------------------------------------------------------------------- 1 | # Mapping 2 | resolution: 0.05 3 | map_frame: "world" 4 | fixed_frame: "odom" 5 | robot_frame: "base_footprint" 6 | offline_octomap_path: "" 7 | # offline_octomap_path: "/home/sasm/pepper_nav_ws/src/pepper_pipeline/map/offline_map_2.bt" 8 | visualize_free_space: True 9 | mapping_max_range: 8 10 | odometry_topic: "/pepper/odom_groundtruth" 11 | 12 | rviz_timer: 1.0 # 0.0 if no visualization 13 | 14 | # pointcloud sensor frame 15 | point_cloud_topics: ["/pepper/camera/depth/points"] 16 | point_cloud_frames: ["/CameraDepth_frame"] 17 | 18 | # social agents config 19 | social_agents_topic: "/pedsim_simulator/simulated_agents" 20 | social_agent_radius: 0.4 21 | 22 | # social relevance validity checking parameters 23 | social_relevance_validity_checking: True 24 | robot_distance_view_max: 8 25 | robot_distance_view_min: 3 26 | robot_angle_view: 1.57 27 | robot_velocity_threshold: 0.35 28 | -------------------------------------------------------------------------------- /esc_move_base_control/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /esc_move_base_planning/config/esc_move_base_planning.yaml: -------------------------------------------------------------------------------- 1 | # Path planning parameters 2 | world_frame: world 3 | planner_name: RRTstar # RRT, RRTstar 4 | planning_bounds_x: [-50.0, 50.0] # [x_min, x_max] 5 | planning_bounds_y: [-50.0, 50.0] # [y_min, y_max] 6 | dynamic_bounds: True # the bounds of the planner will change according to the start and goal configurations 7 | start_state: [0.0, 0.0, 0.0] # [X, Y, Yaw] 8 | goal_state: [5.0, -1.0, 0.0] # [X, Y, Yaw] 9 | timer_period: 1.0 10 | solving_time: 0.9 11 | opport_collision_check: True 12 | reuse_last_best_solution: True 13 | optimization_objective: ExtendedSocialComfort 14 | motion_cost_interpolation: False 15 | xy_goal_tolerance: 0.25 16 | visualize_tree: False 17 | 18 | # Collision checking 19 | grid_map_service: /esc_move_base_mapping/get_grid_map 20 | 21 | robot_base_radius: 0.35 22 | 23 | control_active_topic: "/control_active_topic" 24 | 25 | odometry_topic: /pepper/odom_groundtruth 26 | query_goal_topic: /esc_move_base_planner/query_goal 27 | 28 | goto_action: /esc_goto_action 29 | -------------------------------------------------------------------------------- /esc_move_base_control/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | # Remove the `skip` decorator once the source file(s) have a copyright header 20 | @pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') 21 | @pytest.mark.copyright 22 | @pytest.mark.linter 23 | def test_copyright(): 24 | rc = main(argv=['.', 'test']) 25 | assert rc == 0, 'Found errors' 26 | -------------------------------------------------------------------------------- /esc_move_base_control/LICENSE: -------------------------------------------------------------------------------- 1 | Permission is hereby granted, free of charge, to any person obtaining a copy 2 | of this software and associated documentation files (the "Software"), to deal 3 | in the Software without restriction, including without limitation the rights 4 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 5 | copies of the Software, and to permit persons to whom the Software is 6 | furnished to do so, subject to the following conditions: 7 | 8 | The above copyright notice and this permission notice shall be included in 9 | all copies or substantial portions of the Software. 10 | 11 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 12 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 13 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 14 | THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 15 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 17 | THE SOFTWARE. 18 | -------------------------------------------------------------------------------- /esc_move_base_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | esc_move_base_control 5 | 0.0.0 6 | TODO: Package description 7 | sasm 8 | MIT 9 | 10 | ament_copyright 11 | ament_flake8 12 | ament_pep257 13 | python3-pytest 14 | 15 | 16 | rclpy 17 | std_msgs 18 | rclpy 19 | std_msgs 20 | rclpy 21 | std_msgs 22 | 23 | geometry_msgs 24 | 25 | esc_move_base_msgs 26 | 27 | 28 | ament_python 29 | 30 | 31 | -------------------------------------------------------------------------------- /esc_move_base_planning/src/state_cost_objective.cpp: -------------------------------------------------------------------------------- 1 | /*! \file state_cost_objective.cpp 2 | * \brief State cost objective. 3 | * 4 | * \date Mar 31, 2015 5 | * \author Juan David Hernandez Vega, juandhv@rice.edu 6 | * 7 | * \details State cost objective. Define different state cost objectives. 8 | * 9 | * Based on Juan D. Hernandez Vega's PhD thesis, University of Girona 10 | * http://hdl.handle.net/10803/457592, http://www.tdx.cat/handle/10803/457592 11 | */ 12 | 13 | #include 14 | 15 | ob::OptimizationObjectivePtr getExtendedSocialComfortObjective(const ob::SpaceInformationPtr &si, 16 | bool motion_cost_interpolation) 17 | { 18 | // ROS_INFO_STREAM("Sending extended social comfort objective"); 19 | return ob::OptimizationObjectivePtr(new ExtendedSocialComfortObjective(si, motion_cost_interpolation)); 20 | } 21 | 22 | ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr &si) 23 | { 24 | return ob::OptimizationObjectivePtr(new ob::PathLengthOptimizationObjective(si)); 25 | } 26 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Steven A. Silva 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /esc_move_base_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(esc_move_base_msgs) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(std_msgs REQUIRED) 12 | find_package(rosidl_default_generators REQUIRED) 13 | find_package(geometry_msgs REQUIRED) 14 | find_package(pedsim_msgs REQUIRED) 15 | 16 | rosidl_generate_interfaces(${PROJECT_NAME} 17 | "msg/Path2D.msg" 18 | "action/Goto2D.action" 19 | "action/GotoRegion2D.action" 20 | DEPENDENCIES geometry_msgs pedsim_msgs 21 | ) 22 | 23 | if(BUILD_TESTING) 24 | find_package(ament_lint_auto REQUIRED) 25 | # the following line skips the linter which checks for copyrights 26 | # comment the line when a copyright and license is added to all source files 27 | set(ament_cmake_copyright_FOUND TRUE) 28 | # the following line skips cpplint (only works in a git repo) 29 | # comment the line when this package is in a git repo and when 30 | # a copyright and license is added to all source files 31 | set(ament_cmake_cpplint_FOUND TRUE) 32 | ament_lint_auto_find_test_dependencies() 33 | endif() 34 | 35 | ament_package() 36 | -------------------------------------------------------------------------------- /esc_move_base_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | esc_move_base_msgs 5 | 0.0.0 6 | The esc_move_base_msgs package 7 | Steven Silva 8 | MIT 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | rclcpp 17 | std_msgs 18 | rclcpp 19 | std_msgs 20 | rclcpp 21 | std_msgs 22 | 23 | pedsim_msgs 24 | geometry_msgs 25 | 26 | rosidl_default_generators 27 | rosidl_default_runtime 28 | rosidl_interface_packages 29 | 30 | action_msgs 31 | 32 | 33 | 34 | ament_cmake 35 | 36 | 37 | -------------------------------------------------------------------------------- /esc_move_base_planning/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | esc_move_base_planning 5 | 0.0.0 6 | The esc_move_base_planning package 7 | Steven Silva 8 | MIT 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | rclcpp 16 | std_msgs 17 | rclcpp 18 | std_msgs 19 | rclcpp 20 | std_msgs 21 | 22 | tf2_ros 23 | 24 | 25 | esc_move_base_msgs 26 | esc_move_base_control 27 | esc_move_base_mapping 28 | 29 | 30 | 31 | pedsim_msgs 32 | 33 | 34 | ompl 35 | 36 | ament_cmake 37 | 38 | 39 | -------------------------------------------------------------------------------- /esc_move_base_mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | esc_move_base_mapping 5 | 0.0.0 6 | The esc_move_base_mapping package 7 | Steven Silva 8 | MIT 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | rclcpp 16 | std_msgs 17 | pcl_ros 18 | rclcpp 19 | std_msgs 20 | pcl_ros 21 | rclcpp 22 | std_msgs 23 | pcl_ros 24 | 25 | 26 | 27 | nav_msgs 28 | esc_move_base_msgs 29 | tf2_ros 30 | 31 | pedsim_msgs 32 | grid_map 33 | 34 | 35 | ament_cmake 36 | 37 | 38 | -------------------------------------------------------------------------------- /esc_move_base_planning/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(esc_move_base_planning) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(std_msgs REQUIRED) 12 | find_package(nav_msgs REQUIRED) 13 | find_package(esc_move_base_msgs REQUIRED) 14 | find_package(pedsim_msgs REQUIRED) 15 | find_package(tf2_ros REQUIRED) 16 | find_package(grid_map_core REQUIRED) 17 | find_package(grid_map_cv REQUIRED) 18 | find_package(grid_map_filters REQUIRED) 19 | find_package(grid_map_msgs REQUIRED) 20 | find_package(grid_map_ros REQUIRED) 21 | find_package(grid_map_sdf REQUIRED) 22 | find_package(grid_map_visualization REQUIRED) 23 | find_package(tf2_geometry_msgs REQUIRED) 24 | find_package(ompl REQUIRED) 25 | find_package(geometry_msgs REQUIRED) 26 | 27 | include_directories( 28 | include 29 | SYSTEM) 30 | 31 | if(BUILD_TESTING) 32 | find_package(ament_lint_auto REQUIRED) 33 | # the following line skips the linter which checks for copyrights 34 | # comment the line when a copyright and license is added to all source files 35 | set(ament_cmake_copyright_FOUND TRUE) 36 | # the following line skips cpplint (only works in a git repo) 37 | # comment the line when this package is in a git repo and when 38 | # a copyright and license is added to all source files 39 | set(ament_cmake_cpplint_FOUND TRUE) 40 | ament_lint_auto_find_test_dependencies() 41 | endif() 42 | 43 | set(dependencies 44 | rclcpp 45 | pedsim_msgs 46 | nav_msgs 47 | esc_move_base_msgs 48 | grid_map_ros 49 | grid_map_msgs 50 | tf2_geometry_msgs 51 | geometry_msgs 52 | OMPL 53 | ) 54 | 55 | add_executable(esc_move_base_planner 56 | src/planning_framework_main.cpp 57 | src/state_validity_checker_grid_map_R2.cpp 58 | src/state_cost_objective.cpp 59 | src/new_state_sampler.cpp 60 | src/planner/RRTstarMod.cpp 61 | ) 62 | ament_target_dependencies(esc_move_base_planner ${dependencies}) 63 | target_link_libraries(esc_move_base_planner ${OMPL_LIBRARIES}) 64 | 65 | install(TARGETS 66 | esc_move_base_planner 67 | DESTINATION lib/${PROJECT_NAME}) 68 | 69 | ament_package() 70 | -------------------------------------------------------------------------------- /esc_move_base_mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(esc_move_base_mapping) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(std_msgs REQUIRED) 12 | find_package(nav_msgs REQUIRED) 13 | find_package(esc_move_base_msgs REQUIRED) 14 | find_package(pedsim_msgs REQUIRED) 15 | find_package(tf2_ros REQUIRED) 16 | find_package(grid_map_core REQUIRED) 17 | find_package(grid_map_cv REQUIRED) 18 | find_package(grid_map_filters REQUIRED) 19 | # find_package(grid_map_loader REQUIRED) 20 | find_package(grid_map_msgs REQUIRED) 21 | find_package(grid_map_octomap REQUIRED) 22 | find_package(grid_map_ros REQUIRED) 23 | # find_package(grid_map_rviz_plugin REQUIRED) 24 | find_package(grid_map_sdf REQUIRED) 25 | find_package(grid_map_visualization REQUIRED) 26 | find_package(tf2_geometry_msgs REQUIRED) 27 | find_package(octomap_msgs REQUIRED) 28 | find_package(OCTOMAP REQUIRED) 29 | find_package(octomap_server REQUIRED) 30 | find_package(pcl_ros REQUIRED) 31 | find_package(laser_geometry REQUIRED) 32 | 33 | include_directories( 34 | include 35 | SYSTEM) 36 | 37 | if(BUILD_TESTING) 38 | find_package(ament_lint_auto REQUIRED) 39 | # the following line skips the linter which checks for copyrights 40 | # comment the line when a copyright and license is added to all source files 41 | set(ament_cmake_copyright_FOUND TRUE) 42 | # the following line skips cpplint (only works in a git repo) 43 | # comment the line when this package is in a git repo and when 44 | # a copyright and license is added to all source files 45 | set(ament_cmake_cpplint_FOUND TRUE) 46 | ament_lint_auto_find_test_dependencies() 47 | endif() 48 | 49 | set(dependencies 50 | rclcpp 51 | pedsim_msgs 52 | nav_msgs 53 | esc_move_base_msgs 54 | grid_map_ros 55 | grid_map_msgs 56 | tf2_geometry_msgs 57 | octomap_msgs 58 | octomap_server 59 | pcl_ros 60 | grid_map_octomap 61 | laser_geometry 62 | ) 63 | 64 | add_executable(esc_move_base_mapper 65 | src/world_modeler.cpp 66 | ) 67 | ament_target_dependencies(esc_move_base_mapper ${dependencies}) 68 | target_link_libraries(esc_move_base_mapper ${OCTOMAP_LIBRARIES}) 69 | 70 | install(TARGETS 71 | esc_move_base_mapper 72 | DESTINATION lib/${PROJECT_NAME}) 73 | 74 | ament_package() 75 | -------------------------------------------------------------------------------- /esc_move_base_planning/include/state_validity_checker_grid_map_R2.h: -------------------------------------------------------------------------------- 1 | /*! \file state_validity_checker_grid_map_R2.hpp 2 | * \brief State validity checker. 3 | * 4 | * \date March 5, 2015 5 | * \author Juan David Hernandez Vega, juandhv@rice.edu 6 | * 7 | * \details Check is a given configuration R2 is collision-free. 8 | * The workspace is represented by an GridMap and collision check is done iterating. 9 | * 10 | * Based on Juan D. Hernandez Vega's PhD thesis, University of Girona 11 | * http://hdl.handle.net/10803/457592, http://www.tdx.cat/handle/10803/457592 12 | */ 13 | 14 | #ifndef OMPL_CONTRIB_STATE_VALIDITY_CHECKER_GRID_MAP_R2_ 15 | #define OMPL_CONTRIB_STATE_VALIDITY_CHECKER_GRID_MAP_R2_ 16 | 17 | // ROS2 18 | #include 19 | // ROS2 messages 20 | #include 21 | #include 22 | #include 23 | 24 | // Standard libraries 25 | #include 26 | #include 27 | #include 28 | 29 | // grid map library 30 | #include 31 | #include 32 | #include 33 | 34 | // OMPL 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | // Boost 42 | #include 43 | #include 44 | #include 45 | 46 | // Eigen 47 | #include 48 | 49 | #include 50 | #include 51 | #include 52 | 53 | // #include 54 | #include 55 | #include 56 | 57 | // ROS-GridMap interface 58 | using grid_map_msgs::srv::GetGridMap; 59 | // Standard namespace 60 | using namespace std; 61 | 62 | // OMPL namespaces 63 | namespace ob = ompl::base; 64 | namespace og = ompl::geometric; 65 | 66 | //! GridMapStateValidityCheckerR2 class. 67 | /*! 68 | GridMap State Validity checker. 69 | Extension of an abstract class used to implement the state validity checker over a grid map. 70 | */ 71 | class GridMapStateValidityCheckerR2 : public ob::StateValidityChecker 72 | { 73 | public: 74 | //! GridMapStateValidityCheckerR2 constructor. 75 | /*! 76 | * Besides of initializing the private attributes, it loads the grid map. 77 | */ 78 | GridMapStateValidityCheckerR2(const ob::SpaceInformationPtr &si, const bool opport_collision_check, 79 | std::vector planning_bounds_x, std::vector planning_bounds_y, grid_map_msgs::msg::GridMap grid_map_msg, const double robot_radius); 80 | 81 | //! GridMapStateValidityCheckerR2 destructor. 82 | /*! 83 | * Destroy the grid map. 84 | */ 85 | ~GridMapStateValidityCheckerR2(); 86 | 87 | //! State validator. 88 | /*! 89 | * Function that verifies if the given state is valid (i.e. is free of collision) iterating the grid map 90 | */ 91 | virtual bool isValid(const ob::State *state) const; 92 | 93 | /* 94 | * Returns the cost value for the integration of the path defined on the equation that defines an extended 95 | * social comfort zone model. 96 | */ 97 | virtual double checkExtendedSocialComfort(const ob::State *state, 98 | const ob::SpaceInformationPtr space) const; 99 | 100 | virtual bool isValidPoint(const ob::State *state) const; 101 | 102 | private: 103 | double grid_map_min_x_, grid_map_min_y_, grid_map_min_z_; 104 | double grid_map_max_x_, grid_map_max_y_, grid_map_max_z_; 105 | std::vector planning_bounds_x_, planning_bounds_y_; 106 | double robot_base_radius_; 107 | std::string grid_map_service_; 108 | grid_map_msgs::msg::GridMap grid_map_msgs_; 109 | grid_map::GridMap grid_map_; 110 | 111 | // cost objective type 112 | std::string optimization_objective; 113 | 114 | bool opport_collision_check_; 115 | 116 | grid_map::Matrix obstacles_grid_map_; 117 | grid_map::Matrix comfort_grid_map_; 118 | }; 119 | 120 | #endif 121 | -------------------------------------------------------------------------------- /esc_move_base_planning/include/state_cost_objective.h: -------------------------------------------------------------------------------- 1 | /*! \file state_cost_objective.hpp 2 | * \brief State cost objective. 3 | * 4 | * \date Mar 31, 2015 5 | * \author Juan David Hernandez Vega, juandhv@rice.edu 6 | * 7 | * \details State cost objective. Define different state cost objectives. 8 | * 9 | * Based on Juan D. Hernandez Vega's PhD thesis, University of Girona 10 | * http://hdl.handle.net/10803/457592, http://www.tdx.cat/handle/10803/457592 11 | */ 12 | 13 | #ifndef OMPL_CONTRIB_STATE_COST_OBJECTIVES_ 14 | #define OMPL_CONTRIB_STATE_COST_OBJECTIVES_ 15 | 16 | // OMPL 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | namespace ob = ompl::base; 27 | 28 | class ExtendedSocialComfortObjective : public ob::StateCostIntegralObjective 29 | { 30 | private: 31 | public: 32 | ExtendedSocialComfortObjective(const ob::SpaceInformationPtr &si, bool enableMotionCostInterpolation) 33 | : ob::StateCostIntegralObjective(si, enableMotionCostInterpolation) 34 | { 35 | // ROS_INFO_STREAM("running extended social comfort objective checker"); 36 | } 37 | 38 | // in case of modifying the cost calculation function here it is where it should be done 39 | 40 | ob::Cost stateCost(const ob::State *s) const 41 | { 42 | std::shared_ptr state_vality_checker = 43 | std::static_pointer_cast(si_->getStateValidityChecker()); 44 | return ob::Cost(state_vality_checker->checkExtendedSocialComfort(s, si_)); 45 | } 46 | 47 | ob::Cost motionCost(const ob::State *s1, const ob::State *s2) const 48 | { 49 | if (interpolateMotionCost_) 50 | { 51 | ob::Cost totalCost = this->identityCost(); 52 | 53 | int nd = si_->getStateSpace()->validSegmentCount(s1, s2); 54 | // nd = int(nd/10); 55 | 56 | ob::State *test1 = si_->cloneState(s1); 57 | ob::Cost prevStateCost = this->stateCost(test1); 58 | if (nd > 1) 59 | { 60 | ob::State *test2 = si_->allocState(); 61 | for (int j = 1; j < nd; ++j) 62 | { 63 | si_->getStateSpace()->interpolate(s1, s2, (double)j / (double)nd, test2); 64 | ob::Cost nextStateCost = this->stateCost(test2); 65 | totalCost = ob::Cost( 66 | totalCost.value() + 67 | this->trapezoid(prevStateCost, nextStateCost, si_->distance(test1, test2)).value()); 68 | std::swap(test1, test2); 69 | prevStateCost = nextStateCost; 70 | } 71 | si_->freeState(test2); 72 | } 73 | 74 | // Lastly, add s2 75 | totalCost = ob::Cost( 76 | totalCost.value() + 77 | this->trapezoid(prevStateCost, this->stateCost(s2), si_->distance(test1, s2)).value()); 78 | 79 | si_->freeState(test1); 80 | 81 | // ROS_INFO_STREAM("Total cost: " << totalCost); 82 | 83 | return totalCost; 84 | } 85 | else 86 | { 87 | // ROS_INFO_STREAM("Trapezoid cost: " << this->trapezoid(this->stateCost(s1), this->stateCost(s2), 88 | // si_->distance(s1, s2))); 89 | return this->trapezoid(this->stateCost(s1), this->stateCost(s2), si_->distance(s1, s2)); 90 | } 91 | } 92 | }; 93 | 94 | /** Return an optimization objective which attempts to steer the robot 95 | away from social agents in order to keep comfort with an extended social comfort model. */ 96 | ob::OptimizationObjectivePtr getExtendedSocialComfortObjective(const ob::SpaceInformationPtr &si, 97 | bool motion_cost_interpolation); 98 | 99 | /** Returns a structure representing the optimization objective to use 100 | for optimal motion planning. This method returns an objective 101 | which attempts to minimize the length in configuration space of 102 | computed paths. */ 103 | ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr &si); 104 | 105 | #endif 106 | -------------------------------------------------------------------------------- /esc_move_base_planning/src/state_validity_checker_grid_map_R2.cpp: -------------------------------------------------------------------------------- 1 | /*! \file state_validity_checker_grid_map_R2.cpp 2 | * \brief State validity checker. 3 | * 4 | * \date November 07, 2022 5 | * \author Juan David Hernandez Vega, HernandezVegaJ@cardiff.ac.uk 6 | * \author Steven Alexander Silva Mendoza, silvas1@cardiff.ac.uk 7 | * 8 | * \details Check is a given configuration R2 is collision-free. 9 | * The workspace is represented by an GridMap and collision check is done by iterating. 10 | * 11 | * Based on Juan D. Hernandez Vega's PhD thesis, University of Girona 12 | * http://hdl.handle.net/10803/457592, http://www.tdx.cat/handle/10803/457592 13 | */ 14 | 15 | #include 16 | 17 | GridMapStateValidityCheckerR2::GridMapStateValidityCheckerR2(const ob::SpaceInformationPtr &si, 18 | const bool opport_collision_check, 19 | std::vector planning_bounds_x, 20 | std::vector planning_bounds_y, grid_map_msgs::msg::GridMap grid_map_msg, const double robot_radius) 21 | : ob::StateValidityChecker(si), robot_base_radius_(0.4) 22 | { 23 | 24 | opport_collision_check_ = opport_collision_check; 25 | planning_bounds_x_ = planning_bounds_x; 26 | planning_bounds_y_ = planning_bounds_y; 27 | robot_base_radius_ = robot_radius; 28 | 29 | if (grid_map::GridMapRosConverter::fromMessage(grid_map_msg, grid_map_)) 30 | { 31 | 32 | grid_map_max_x_ = grid_map_msg.info.pose.position.x + (grid_map_msg.info.length_x / 2); 33 | grid_map_min_x_ = grid_map_msg.info.pose.position.x - (grid_map_msg.info.length_x / 2); 34 | 35 | grid_map_max_y_ = grid_map_msg.info.pose.position.y + (grid_map_msg.info.length_y / 2); 36 | grid_map_min_y_ = grid_map_msg.info.pose.position.y - (grid_map_msg.info.length_y / 2); 37 | } 38 | 39 | try 40 | { 41 | obstacles_grid_map_ = grid_map_["full"]; 42 | comfort_grid_map_ = grid_map_["comfort"]; 43 | } 44 | catch (...) 45 | { 46 | } 47 | } 48 | 49 | bool GridMapStateValidityCheckerR2::isValid(const ob::State *state) const 50 | { 51 | const ob::RealVectorStateSpace::StateType *state_r2 = state->as(); 52 | 53 | // ompl::tools::Profiler::Begin("collision"); 54 | 55 | // extract the component of the state and cast it to what we expect 56 | 57 | if (opport_collision_check_ && 58 | (state_r2->values[0] < grid_map_min_x_ || state_r2->values[1] < grid_map_min_y_ || 59 | state_r2->values[0] > grid_map_max_x_ || state_r2->values[1] > grid_map_max_y_)) 60 | { 61 | // ompl::tools::Profiler::End("collision"); 62 | return true; 63 | } 64 | 65 | if (state_r2->values[0] < planning_bounds_x_[0] || state_r2->values[1] < planning_bounds_y_[0] || 66 | state_r2->values[0] > planning_bounds_x_[1] || state_r2->values[1] > planning_bounds_y_[1]) 67 | { 68 | // ompl::tools::Profiler::End("collision"); 69 | return false; 70 | } 71 | 72 | grid_map::Position query(state_r2->values[0], state_r2->values[1]); 73 | 74 | for (grid_map::CircleIterator iterator(grid_map_, query, robot_base_radius_); 75 | !iterator.isPastEnd(); ++iterator) 76 | { 77 | const grid_map::Index index(*iterator); 78 | 79 | if (obstacles_grid_map_(index(0), index(1)) > 20) 80 | { 81 | return false; 82 | } 83 | } 84 | 85 | return true; 86 | } 87 | 88 | double GridMapStateValidityCheckerR2::checkExtendedSocialComfort(const ob::State *state, 89 | const ob::SpaceInformationPtr space) const 90 | { 91 | double state_risk = 0.0; 92 | 93 | const ob::RealVectorStateSpace::StateType *state_r2 = state->as(); 94 | 95 | grid_map::Position query(state_r2->values[0], state_r2->values[1]); 96 | 97 | grid_map::Index index; 98 | 99 | if (grid_map_.getIndex(query, index)) 100 | { 101 | state_risk = comfort_grid_map_(index(0), index(1)); 102 | } 103 | 104 | if (state_risk < 1 || isnan(state_risk)) 105 | { 106 | state_risk = 1; 107 | } 108 | 109 | return state_risk; 110 | } 111 | 112 | bool GridMapStateValidityCheckerR2::isValidPoint(const ob::State *state) const 113 | { 114 | 115 | // extract the component of the state and cast it to what we expect 116 | const ob::RealVectorStateSpace::StateType *state_r2 = state->as(); 117 | 118 | grid_map::Position query(state_r2->values[0], state_r2->values[1]); 119 | 120 | grid_map::Index index; 121 | 122 | if (grid_map_.getIndex(query, index)) 123 | { 124 | if (obstacles_grid_map_(index(0), index(1)) > 20) 125 | { 126 | return false; 127 | } 128 | } 129 | 130 | return true; 131 | } 132 | 133 | GridMapStateValidityCheckerR2::~GridMapStateValidityCheckerR2() 134 | { 135 | } 136 | -------------------------------------------------------------------------------- /esc_move_base_planning/src/new_state_sampler.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014, Rice University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Rice University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | * Modified. Based on Juan D. Hernandez Vega's PhD thesis, University of Girona 35 | * http://hdl.handle.net/10803/457592, http://www.tdx.cat/handle/10803/457592 36 | *********************************************************************/ 37 | 38 | /* Author: Javier V. Gómez*/ 39 | 40 | #include 41 | #include 42 | 43 | ob::StateSamplerPtr newAllocStateSampler(const ob::StateSpace *space, const ob::PlannerPtr &planner, 44 | const std::vector &start_states) 45 | { 46 | NewStateSampler *sampler = new NewStateSampler(space, space->allocDefaultStateSampler()); 47 | // std::cout << "start_states.size(): " << start_states.size() << std::endl; 48 | // std::cout << "bias: " << planner->as()->getGoalBias() << std::endl; 49 | sampler->setStatesToSample(start_states); 50 | sampler->setPlanner(planner); 51 | return ob::StateSamplerPtr(sampler); 52 | } 53 | 54 | void NewStateSampler::setPlanner(const ob::PlannerPtr &planner) 55 | { 56 | planner_ = planner; 57 | } 58 | 59 | void NewStateSampler::sampleUniform(ob::State *state) 60 | { 61 | // std::cout << "statesToSample_.size(): " << statesToSample_.size() << std::endl; 62 | if (!statesToSample_.empty()) 63 | { 64 | // simple_setup_->get()->getPlanner()->as()->setGoalBias(0.0); 65 | planner_->as()->setGoalBias(0.0); 66 | getNextSample(state); 67 | // std::cout << "reusing state: " << state->as()->getX() << ", " 68 | // << state->as()->getY() << std::endl; std::cout << "bias: " << 69 | // planner_->as()->getGoalBias() << std::endl; 70 | } 71 | else 72 | { // if(!reusing_){ 73 | // simple_setup_->get()->getPlanner()->as();//->setGoalBias(0.05); 74 | planner_->as()->setGoalBias(0.05); 75 | sampler_->sampleUniform(state); 76 | // std::cout << "state: " << state->as()->getX() << ", " << 77 | //state->as()->getY() << std::endl; std::cout << "bias: " << 78 | // planner_->as()->getGoalBias() << std::endl; 79 | } 80 | // else 81 | // std::cout << "reusing state: " << state->as()->values[0] << ", 82 | //" << state->as()->values[1] << std::endl; 83 | } 84 | 85 | void NewStateSampler::sampleUniformNear(ob::State *state, const ob::State *near, const double distance) 86 | { 87 | if (!statesToSample_.empty()) 88 | getNextSample(state); 89 | else 90 | sampler_->sampleUniformNear(state, near, distance); 91 | } 92 | 93 | void NewStateSampler::sampleGaussian(ob::State *state, const ob::State *mean, const double stdDev) 94 | { 95 | if (!statesToSample_.empty()) 96 | getNextSample(state); 97 | else 98 | sampler_->sampleGaussian(state, mean, stdDev); 99 | } 100 | 101 | void NewStateSampler::setStatesToSample(const std::vector &states) 102 | { 103 | // boost::mutex::scoped_lock slock(statesLock_); 104 | for (size_t i = 0; i < statesToSample_.size(); ++i) 105 | space_->freeState(statesToSample_[i]); 106 | statesToSample_.clear(); 107 | 108 | statesToSample_.reserve(states.size()); 109 | // std::cout << "state to be recovered: " << states.size() << std::endl; 110 | // push in reverse order, so that the states are popped in order in getNextSample() 111 | // for (std::vector::const_iterator st = states.begin(); st != states.end(); ++st) 112 | reusing_ = false; 113 | for (size_t i = 0; i < states.size(); i++) 114 | { 115 | reusing_ = true; 116 | ob::State *s = space_->allocState(); 117 | space_->copyState(s, states[i]); 118 | statesToSample_.push_back(s); 119 | // std::cout << "recovering state: " << s->as()->values[0] << ", 120 | // " << s->as()->values[1] << std::endl; 121 | } 122 | } 123 | 124 | void NewStateSampler::getNextSample(ob::State *state) 125 | { 126 | // boost::mutex::scoped_lock slock(statesLock_); 127 | space_->copyState(state, statesToSample_.back()); 128 | space_->freeState(statesToSample_.back()); 129 | statesToSample_.pop_back(); 130 | } 131 | 132 | void NewStateSampler::clear() 133 | { 134 | // boost::mutex::scoped_lock slock(statesLock_); 135 | for (size_t i = 0; i < statesToSample_.size(); ++i) 136 | space_->freeState(statesToSample_[i]); 137 | statesToSample_.clear(); 138 | sampler_.reset(); 139 | } 140 | 141 | //------- 142 | -------------------------------------------------------------------------------- /esc_move_base_mapping/include/world_modeler.hpp: -------------------------------------------------------------------------------- 1 | #ifndef WORLD_MODELER_HPP 2 | #define WORLD_MODELER_HPP 3 | 4 | // ROS2 5 | #include 6 | 7 | // ROS2 messages 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | // ROS2 services 20 | #include 21 | 22 | // ROS2 LaserScan tools 23 | #include 24 | 25 | // tf2 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include "message_filters/subscriber.h" 31 | #include 32 | 33 | // Octomap 34 | #include 35 | #include 36 | #include 37 | typedef octomap_msgs::srv::GetOctomap OctomapSrv; 38 | #include 39 | 40 | // grid map library 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | // PCL 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | 63 | // PEDSIM 64 | #include 65 | 66 | #include 67 | 68 | typedef pcl::PointXYZ PCLPoint; 69 | typedef pcl::PointCloud PCLPointCloud; 70 | typedef octomap::OcTree OcTreeT; 71 | 72 | void stopNode(int sig) 73 | { 74 | rclcpp::shutdown(); 75 | exit(0); 76 | } 77 | 78 | //! WorldModeler class. 79 | /*! 80 | * Autopilot Laser WorldModeler. 81 | * Create a WorldModeler using information from laser scans. 82 | */ 83 | class WorldModeler : public rclcpp::Node 84 | { 85 | public: 86 | //! Constructor 87 | WorldModeler(); 88 | //! Destructor 89 | virtual ~WorldModeler(); 90 | //! Callback for getting the laser_scan data 91 | void laserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr laser_scan_msg); 92 | //! Callback for getting the point_cloud data 93 | void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud); 94 | //! Callback for getting current vehicle odometry 95 | void odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg); 96 | //! Callback for getting current agent states 97 | void agentStatesCallback(const pedsim_msgs::msg::AgentStates::SharedPtr agent_states_msg); 98 | bool isAgentInRFOV(const pedsim_msgs::msg::AgentState agent_state); 99 | //! Check if robot is in front of agent. 100 | bool isRobotInFront(pedsim_msgs::msg::AgentState agent_state, grid_map::Position position); 101 | //! Calculate comfort in specific position in gridmap 102 | double getExtendedPersonalSpace(pedsim_msgs::msg::AgentState agent_state, grid_map::Position position); 103 | //! Periodic callback to publish the map for visualization. 104 | void timerCallback(); 105 | void insertScan(const geometry_msgs::msg::Vector3 &sensorOriginTf, const PCLPointCloud &ground, 106 | const PCLPointCloud &nonground, const double &max_range, const double &min_range); 107 | //! Service to save a binary Octomap (.bt) 108 | bool saveBinaryOctomapSrv(const std::shared_ptr req, 109 | std::shared_ptr res); 110 | //! Service to save a full Octomap (.ot) 111 | bool saveFullOctomapSrv(const std::shared_ptr req, 112 | std::shared_ptr res); 113 | //! Service to get a binary Octomap 114 | bool getBinaryOctomapSrv(const std::shared_ptr req, 115 | std::shared_ptr res); 116 | //! Service to get grid map 117 | bool getGridMapSrv(const std::shared_ptr req, 118 | std::shared_ptr res); 119 | 120 | //! Publish the WorldModeler 121 | void publishMap(); 122 | //! Redefine the gridmap with octomap 123 | void defineSocialGridMap(); 124 | 125 | private: 126 | //! Filter outliers 127 | void filterSingleOutliers(sensor_msgs::msg::LaserScan &laser_scan_msg, 128 | std::vector &rngflags); 129 | 130 | // ROS2 131 | rclcpp::Publisher::SharedPtr octomap_marker_pub_; 132 | rclcpp::Publisher::SharedPtr grid_map_pub_; 133 | rclcpp::Subscription::SharedPtr odom_sub_; 134 | rclcpp::Subscription::SharedPtr agent_states_sub_; 135 | std::shared_ptr> point_cloud_sub_; 136 | std::shared_ptr> laser_scan_sub_; 137 | 138 | // SERVICES 139 | rclcpp::Service::SharedPtr save_binary_octomap_srv_; 140 | rclcpp::Service::SharedPtr save_full_octomap_srv_; 141 | rclcpp::Service::SharedPtr get_binary_octomap_srv_; 142 | rclcpp::Service::SharedPtr get_grid_map_srv_; 143 | 144 | rclcpp::TimerBase::SharedPtr timer_; 145 | std::shared_ptr> point_cloud_mn_; 146 | std::shared_ptr> laser_scan_mn_; 147 | 148 | // tf2 149 | std::shared_ptr tf_listener_{nullptr}; 150 | std::shared_ptr tf_buffer_; 151 | 152 | // Names 153 | std::string map_frame_, fixed_frame_, robot_frame_, offline_octomap_path_, 154 | odometry_topic_, social_agents_topic_; 155 | 156 | // Laser scans 157 | std::string laser_scan_frame_, laser_scan_topic_; 158 | 159 | // Point Clouds 160 | std::string point_cloud_topic_, point_cloud_frame_; 161 | 162 | // ROS Messages 163 | sensor_msgs::msg::PointCloud2 cloud_; 164 | 165 | nav_msgs::msg::Odometry::SharedPtr robot_odometry_; 166 | 167 | // pedsim messages 168 | pedsim_msgs::msg::AgentStates::SharedPtr agent_states_; 169 | pedsim_msgs::msg::AgentStates relevant_agent_states_; 170 | 171 | pedsim_msgs::msg::AgentStates social_agents_in_radius_; 172 | std::vector social_agents_in_radius_vector_; 173 | 174 | double social_agent_radius_; 175 | 176 | // Octree 177 | octomap::OcTree *octree_; 178 | double octree_resol_, minimum_range_, rviz_timer_; 179 | octomap::OcTreeKey m_updateBBXMin; 180 | octomap::OcTreeKey m_updateBBXMax; 181 | octomap::KeyRay m_keyRay; // temp storage for ray casting 182 | double mapping_max_range_, min_z_pc_, max_z_pc_; 183 | 184 | // grid map 185 | grid_map::GridMap grid_map_; 186 | 187 | grid_map_msgs::msg::GridMap grid_map_msg_; 188 | 189 | // social relevance validity checking constants 190 | double robot_distance_view_max_, robot_distance_view_min_, robot_velocity_threshold_, robot_angle_view_, actual_fov_distance_; 191 | 192 | // Basic social personal space parameters defined 193 | double social_comfort_amplitude_ = 4; 194 | double sigma_x = 0.45; 195 | double sigma_y = 0.45; 196 | double fv = 0.8; 197 | double fFront = 0.2; 198 | double fFieldOfView = 0.0; 199 | 200 | // Flags 201 | tf2::Vector3 prev_map_to_fixed_pos_; 202 | bool initialized_; 203 | bool nav_sts_available_; 204 | bool visualize_free_space_; 205 | bool social_relevance_validity_checking_; 206 | 207 | bool apply_filter_; 208 | bool add_max_ranges_; 209 | bool add_rays_; 210 | 211 | double orientation_drift_, prev_map_to_fixed_yaw_, position_drift_; 212 | 213 | // LaserScan => (x,y,z) 214 | laser_geometry::LaserProjection laser_scan_projector_; 215 | 216 | protected: 217 | inline static void updateMinKey(const octomap::OcTreeKey &in, 218 | octomap::OcTreeKey &min) 219 | { 220 | for (unsigned i = 0; i < 3; ++i) 221 | min[i] = std::min(in[i], min[i]); 222 | }; 223 | inline static void updateMaxKey(const octomap::OcTreeKey &in, 224 | octomap::OcTreeKey &max) 225 | { 226 | for (unsigned i = 0; i < 3; ++i) 227 | max[i] = std::max(in[i], max[i]); 228 | }; 229 | }; 230 | 231 | #endif // WORLD_MODELER_HPP 232 | -------------------------------------------------------------------------------- /esc_move_base_planning/include/new_state_sampler.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014, Rice University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Rice University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | * Based on Juan D. Hernandez Vega's PhD thesis, University of Girona 35 | * http://hdl.handle.net/10803/457592, http://www.tdx.cat/handle/10803/457592 36 | *********************************************************************/ 37 | 38 | /* Author: Javier V. Gómez */ 39 | 40 | #ifndef OMPL_BASE_SAMPLERS_NEW_STATE_SAMPLER_ 41 | #define OMPL_BASE_SAMPLERS_NEW_STATE_SAMPLER_ 42 | 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | // Eigen 55 | #include 56 | 57 | // OMPL namespaces 58 | namespace ob = ompl::base; 59 | namespace oc = ompl::control; 60 | namespace og = ompl::geometric; 61 | 62 | ob::StateSamplerPtr newAllocStateSampler(const ob::StateSpace *space, const ob::PlannerPtr &planner, 63 | const std::vector &start_states); 64 | 65 | /** \brief Extended state sampler to use with the CForest planning algorithm. It wraps the user-specified 66 | state sampler.*/ 67 | class NewStateSampler : public ob::StateSampler 68 | { 69 | public: 70 | /** \brief Constructor */ 71 | NewStateSampler(const ob::StateSpace *space, ob::StateSamplerPtr sampler) 72 | : ob::StateSampler(space), sampler_(sampler) 73 | { 74 | } 75 | 76 | /** \brief Destructor */ 77 | ~NewStateSampler() 78 | { 79 | clear(); 80 | } 81 | 82 | /** \brief It will sample the next state of the vector StatesToSample_. If this is empty, 83 | it will call the sampleUniform() method of the specified sampler. */ 84 | virtual void sampleUniform(ob::State *state); 85 | 86 | /** \brief It will sample the next state of the vector StatesToSample_. If this is empty, 87 | it will call the sampleUniformNear() method of the specified sampler. */ 88 | virtual void sampleUniformNear(ob::State *state, const ob::State *near, const double distance); 89 | 90 | /** \brief It will sample the next state of the vector StatesToSample_. If this is empty, 91 | it will call the sampleGaussian() method of the specified sampler. */ 92 | virtual void sampleGaussian(ob::State *state, const ob::State *mean, const double stdDev); 93 | 94 | const ob::StateSpace *getStateSpace() const 95 | { 96 | return space_; 97 | } 98 | 99 | /** \brief Fills the vector StatesToSample_ of states to be sampled in the next 100 | calls to sampleUniform(), sampleUniformNear() or sampleGaussian(). */ 101 | void setStatesToSample(const std::vector &states); 102 | 103 | void setPlanner(const ob::PlannerPtr &planner); 104 | 105 | void clear(); 106 | 107 | protected: 108 | /** \brief Extracts the next sample when statesToSample_ is not empty. */ 109 | void getNextSample(ob::State *state); 110 | 111 | /** \brief States to be sampled */ 112 | std::vector statesToSample_; 113 | 114 | /** \brief Underlying, user-specified state sampler. */ 115 | ob::StateSamplerPtr sampler_; 116 | 117 | ob::PlannerPtr planner_; 118 | 119 | /** \brief Lock to control the access to the statesToSample_ vector. */ 120 | // boost::mutex statesLock_; 121 | bool reusing_; 122 | }; 123 | 124 | //------ 125 | 126 | oc::ControlSamplerPtr newAllocControlSampler(const oc::ControlSpace *cspace, 127 | const std::vector &start_controls); 128 | 129 | /** \brief Extended state sampler to use with the CForest planning algorithm. It wraps the user-specified 130 | state sampler.*/ 131 | class NewControlSampler : public oc::ControlSampler 132 | { 133 | public: 134 | /** \brief Constructor */ 135 | NewControlSampler(const oc::ControlSpace *cspace, oc::ControlSamplerPtr control_sampler) 136 | : oc::ControlSampler(cspace), csampler_(control_sampler) 137 | { 138 | } 139 | 140 | /** \brief Destructor */ 141 | ~NewControlSampler() 142 | { 143 | clear(); 144 | } 145 | 146 | /** \brief Sample a control. All other control sampling functions default to this one, 147 | unless a user-specified implementation is given. */ 148 | virtual void sample(oc::Control *control); 149 | 150 | /** \brief Sample a control, given it is applied to a specific state (state). The default 151 | implementation calls the previous definition of sample(). Providing a different 152 | implementation of this function is useful if, for example, the sampling of controls 153 | depends on the state of the system. When attempting to sample controls that keep a 154 | system stable, for example, knowing the state at which the control is applied is important. */ 155 | virtual void sample(oc::Control *control, const ob::State *state); 156 | 157 | /** \brief Sample a control, given the previously applied control. The default implementation 158 | calls the first definition of sample(). For some systems it is possible that large changes 159 | in controls are not desirable. For example, switching from maximum acceleration to maximum 160 | deceleration is not desirable when driving a car. */ 161 | virtual void sampleNext(oc::Control *control, const oc::Control *previous); 162 | 163 | /** \brief Sample a control, given the previously applied control and that it is applied to a 164 | specific state. The default implementation calls the first definition of sample(), even if 165 | other implementations of the sampleNext() shown above are provided. Often this function needs 166 | to be overridden as it is the function planners typically call. */ 167 | virtual void sampleNext(oc::Control *control, const oc::Control *previous, const ob::State *state); 168 | 169 | /** \brief Fills the vector ControlsToSample_ of controls to be sampled in the next 170 | calls to sample(), sampleNext() or sampleGaussian(). */ 171 | void setControlsToSample(const std::vector &controls); 172 | 173 | void clear(); 174 | 175 | protected: 176 | /** \brief Extracts the next sample when controlsToSample_ is not empty. */ 177 | void getNextControl(oc::Control *control); 178 | 179 | /** \brief Controls to be sampled */ 180 | std::vector controlsToSample_; 181 | 182 | /** \brief Underlying, user-specified control sampler. */ 183 | oc::ControlSamplerPtr csampler_; 184 | 185 | /** \brief Lock to control the access to the statesToSample_ vector. */ 186 | // boost::mutex statesLock_; 187 | }; 188 | 189 | //------ 190 | 191 | oc::DirectedControlSamplerPtr newAllocDirectedControlSampler(const oc::SpaceInformation *si); 192 | 193 | class NewDirectedControlSampler : public oc::DirectedControlSampler 194 | { 195 | public: 196 | NewDirectedControlSampler(const oc::SpaceInformation *si); 197 | 198 | virtual ~NewDirectedControlSampler(); 199 | 200 | unsigned int getNumControlSamples() const 201 | { 202 | return numControlSamples_; 203 | } 204 | 205 | void setNumControlSamples(unsigned int numSamples) 206 | { 207 | numControlSamples_ = numSamples; 208 | } 209 | 210 | virtual unsigned int sampleTo(oc::Control *control, const ob::State *source, ob::State *dest); 211 | 212 | virtual unsigned int sampleTo(oc::Control *control, const oc::Control *previous, const ob::State *source, 213 | ob::State *dest); 214 | 215 | protected: 216 | virtual unsigned int getBestControl(oc::Control *control, const ob::State *source, ob::State *dest, 217 | const oc::Control *previous); 218 | unsigned int propagateWhileValid(const ob::State *state, const ob::State *dest, int steps, 219 | ob::State *result) const; 220 | 221 | oc::ControlSamplerPtr cs_; 222 | 223 | unsigned int numControlSamples_; 224 | }; 225 | 226 | void DynUnicycle(const ob::State *start, const ob::State *desired, const double duration, ob::State *result); 227 | 228 | #endif 229 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Extended Social Comfort Robot Navigation Framework 2 | 3 | This is an online social robot navigation framework for indoor social scenarios. From this work a paper conference was submitted [Towards Online Socially Acceptable Robot Navigation](https://ieeexplore.ieee.org/document/9926686). Check the [Cite Our Paper](#cite-our-paper) in case you use this framework for your research or work. 4 | 5 | It is composed of four different packages: 6 | 7 | - `esc_move_base_control`: it is a simple differential control method. 8 | - `esc_move_base_mapping`: is in charge of the perception of the space. 9 | - `esc_move_base_msgs`: contains the messages needed for the framework and the start-goal queries. 10 | - `esc_move_base_planning`: responsible for finding solution paths for the navigation query requested. 11 | 12 | To understand more about how the framework works, you are highly encouraged to take a look at the paper. 13 | 14 | To run the framework, there is a launch file example in every package with example configurations. 15 | 16 | ![FrameworkConnections](https://i.imgur.com/0YOMmiD.png) 17 | 18 | - [Extended Social Comfort Robot Navigation Framework](#extended-social-comfort-robot-navigation-framework) 19 | - [World Modeling (`esc_move_base_mapping`)](#world-modeling-esc_move_base_mapping) 20 | - [Parameters](#parameters) 21 | - [Subscribers](#subscribers) 22 | - [Publishers](#publishers) 23 | - [Services](#services) 24 | - [Online Social Robot Path Planning (`esc_move_base_planning`)](#online-social-robot-path-planning-esc_move_base_planning) 25 | - [Parameters](#parameters-1) 26 | - [Subscribers](#subscribers-1) 27 | - [Publishers](#publishers-1) 28 | - [Actions](#actions) 29 | - [Path Following Control (`esc_move_base_control`)](#path-following-control-esc_move_base_control) 30 | - [Parameters](#parameters-2) 31 | - [Subscribers](#subscribers-2) 32 | - [Publishers](#publishers-2) 33 | - [Cite Our Paper](#cite-our-paper) 34 | 35 | ## World Modeling (`esc_move_base_mapping`) 36 | 37 | This package is in charge of the perception for the robot. For that, a depth camera is used, which helps to generate a 3D map of the space by using Octomap. 38 | 39 | This 3D map is combined in a multilayer GridMap with the position of the social agents detected in the space. 40 | 41 | ### Parameters 42 | 43 | - resolution (double, default: 1.0) 44 | 45 | Octomap and Grid Map resolution. 46 | 47 | - map_frame (string, default: "map") 48 | 49 | - fixed_frame (string, default: "fixed_frame") 50 | 51 | Fixed frame considered for the mapping, can be `odom` for example. 52 | 53 | - robot_frame (string, default: "/robot_frame") 54 | 55 | Base frame considered from the robot for the mapping. 56 | 57 | - oflline_octomap_path (string, default: "") 58 | 59 | If defined, then the given map is used and no ther mapping is done. 60 | 61 | - visualize_free_space (bool, default: True) 62 | 63 | Wether you would like to see the 3D map and grid_map. If `False`, nothing is published at `/esc_move_base_mapping/social_grid_map` and `/esc_move_base_mapping/octomap_map`. 64 | 65 | - mapping_max_range (double, default: 5.0) 66 | 67 | Maximum distance to generate the 3D map from the obtained depth points. 68 | 69 | - odometry_topic (string, default: "/odometry_topic") 70 | 71 | - rviz_timer (double, default: 0.0) 72 | 73 | The time delay to publish messages to RViz. Not used if `visualize_free_space` is `False`. 74 | 75 | - point_cloud_topics (list: string, default: empty) 76 | 77 | List of pointcloud topics used to generate the 3D map. 78 | 79 | - point_cloud_frames (list: string, default: empty) 80 | 81 | List of frames from pointcloud topics in order respectively. 82 | 83 | - social_agents_topic (string, default: "/pedsim_simulator/simulated_agents") 84 | 85 | Topic with social agents states. 86 | 87 | - social_agent_radius (double, default: 0.4) 88 | 89 | Radius considered for the social agents. 90 | 91 | - social_relevance_validity_checking (bool, default: False) 92 | 93 | Wether or not it is desired to only consider the relevant agents. 94 | 95 | - robot_distance_view_max (double, default: 6.0) 96 | 97 | Maximum distance for the robot to consider social agents. 98 | 99 | - robot_distance_view_min (double, default: 1.5) 100 | 101 | Mininum distance for the robot to consider social agents. 102 | 103 | - robot_angle_view (double, default: 1.57) 104 | 105 | Maximum angle in which the social agents are considered from the field of view of the robot. Defined in radians. 106 | 107 | - robot_velocity_threshold (double, default: 0.3) 108 | 109 | Maximum velocity that the robot can have. 110 | 111 | ### Subscribers 112 | 113 | The name of the subscribers' topics are just defined as an example, but they may be configured using the parameters defined before. 114 | 115 | - /pedsim_simulator/simulated_agents ([pedsim_msgs/AgentStates](https://github.com/CardiffUniversityComputationalRobotics/pedsim_ros/blob/noetic-devel/pedsim_msgs/msg/AgentStates.msg)) 116 | 117 | Position, orientation, velocity and other states of the social agents. 118 | 119 | - /pepper/camera/depth/points ([sensor_msgs/PointCloud2](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointCloud2.html)) 120 | 121 | Depth points from depth camera. 122 | 123 | - /pepper/odom_groundtruth ([nav_msgs/Odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html)) 124 | 125 | Robot odometry (in this case it was Pepper robot). 126 | 127 | ### Publishers 128 | 129 | The name of the publishers' topics are just defined as an example, but they may be configured using the parameters defined before. 130 | 131 | - /esc_move_base_mapping/octomap_map ([visualization_msgs/MarkerArray](http://docs.ros.org/en/noetic/api/visualization_msgs/html/msg/MarkerArray.html)) 132 | 133 | Visual representation of the 3D map created. 134 | 135 | - /esc_move_base_mapping/relevant_agents ([pedsim_msgs/AgentStates](https://github.com/CardiffUniversityComputationalRobotics/pedsim_ros/blob/noetic-devel/pedsim_msgs/msg/AgentStates.msg)) 136 | 137 | If parameter `social_relevance_validity_checking` is `true`, then only the filtered relevant agents are published. Otherwise, all agents are re-published. 138 | 139 | - /esc_move_base_mapping/social_grid_map ([grid_map_msgs/GridMap](http://docs.ros.org/en/kinetic/api/grid_map_msgs/html/msg/GridMap.html)) 140 | 141 | Grid map with 2 layers, one for obstacles and another for social agents. 142 | 143 | ### Services 144 | 145 | - /esc_move_base_mapping/get_grid_map ([grid_map_msgs/GetGridMap](http://docs.ros.org/en/indigo/api/grid_map_msgs/html/srv/GetGridMap.html)) 146 | 147 | Fetches the grid map from the World Modeling. 148 | 149 | ## Online Social Robot Path Planning (`esc_move_base_planning`) 150 | 151 | This package is in charge of finding socially acceptable solution paths for the robot to follow. 152 | 153 | ### Parameters 154 | 155 | All of the following parameters have to be defined since they have no default values. 156 | 157 | - world_frame (string) 158 | 159 | Main parent frame to be used as reference. 160 | 161 | - planner_name (string) 162 | 163 | Planner to be used, can be RRT, RRTstar or PRMstar. 164 | 165 | - planning_bounds_x (list: [x_min, x_max]) 166 | 167 | Limits for planning in the X axis. 168 | 169 | - planning_bounds_y (list: [y_min, y_max]) 170 | 171 | Limits for planning in the Y axis. 172 | 173 | - dynamic_bounds (bool) 174 | 175 | Whether it is wanted to change the planning bounds dinamically depending on the navigation query. 176 | 177 | - start_state (list, [X, Y, Yaw]) 178 | 179 | Define start state for navigation query if the action is not going to be used. 180 | 181 | - goal_state (list, [X, Y, Yaw]) 182 | 183 | Define goal state for navigation query if no action is used. 184 | 185 | - timer_period (double) 186 | 187 | Controls the time for sleeps in the node. 188 | 189 | - solving_time (double) 190 | 191 | Time given to the planner to solve the navigation query. 192 | 193 | - opport_collision_check (bool) 194 | 195 | Whether it is wanted to use opportunistic collision checking or not. 196 | 197 | - reuse_last_best_solution (bool) 198 | 199 | Whether it is wanted to apply the reuse of the last best known solution. 200 | 201 | - optimization_objective (string) 202 | 203 | Defines which optimization objective to use is desired. Currently, only supported PathLength and ExtendedSocialComfort. 204 | 205 | - motion_cost_interpolation (bool) 206 | 207 | If motion cost interpolation is desired. 208 | 209 | - xy_goal_tolerance (double) 210 | 211 | Tolerance in the XY axis for the goal state. 212 | 213 | - visualize_tree: (bool) 214 | 215 | Whether it is desired to visualize or not the tree generated by the planner. 216 | 217 | - grid_map_service (string) 218 | 219 | Service name to get the GridMap. 220 | 221 | - control_active_topic (string) 222 | 223 | Topic to communicate with the control module to know when control is ready. 224 | 225 | - robot_base_radius (double) 226 | 227 | Radius of the base of the robot to be considered for the planning. 228 | 229 | - odometry_topic (string) 230 | 231 | - query_goal_topic (string) 232 | 233 | Topic to request a query without using actions. 234 | 235 | - goto_action (string) 236 | 237 | Action name to run a navigation query. 238 | 239 | ### Subscribers 240 | 241 | The name of the subscribers' topics are just defined as an example, but they may be configured using the parameters defined before. 242 | 243 | - /esc_move_base_planner/query_goal ([geometry_msgs/PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)) 244 | - /pepper/odom_groundtruth ([nav_msgs/Odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html)) 245 | 246 | Robot odometry. 247 | 248 | - /esc_move_base_mapping/relevant_agents ([pedsim_msgs/AgentStates](https://github.com/CardiffUniversityComputationalRobotics/pedsim_ros/blob/noetic-devel/pedsim_msgs/msg/AgentStates.msg)) 249 | 250 | Agents considered by the mapping module. 251 | 252 | - /control_active_topic ([std_msgs/Bool](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Bool.html)) 253 | 254 | ### Publishers 255 | 256 | The name of the publishers' topics are just defined as an example, but they may be configured using the parameters defined before. 257 | 258 | - /esc_move_base_planner/esc_move_base_solution_path ([esc_move_base_msgs/Path2D](https://github.com/CardiffUniversityComputationalRobotics/esc-nav-stack/blob/world_modeling/esc_move_base_msgs/msg/Path2D.msg)) 259 | 260 | Solution path found by the planner. It is passed to the control module. 261 | 262 | - /esc_move_base_planner/esc_num_nodes ([std_msgs/Int32](http://docs.ros.org/en/melodic/api/std_msgs/html/msg/Int32.html)) 263 | 264 | Number of valid nodes sampled by the planner. 265 | 266 | - /esc_move_base_planner/query_goal_pose_rviz ([geometry_msgs/PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)) 267 | 268 | Shows in RViz the query pose requested to the planner. 269 | 270 | - /esc_move_base_planner/query_goal_radius_rviz ([visualization_msgs/Marker](http://docs.ros.org/en/noetic/api/visualization_msgs/html/msg/Marker.html)) 271 | 272 | Shows in RViz the radius of the query requested to the planner. 273 | 274 | - /esc_move_base_planner/solution_path ([visualization_msgs/Marker](http://docs.ros.org/en/noetic/api/visualization_msgs/html/msg/Marker.html)) 275 | 276 | Visual solution path to be seen in RViz. 277 | 278 | ### Actions 279 | 280 | - `/goto_action` ([esc_move_base_msgs/GoTo2D](https://github.com/CardiffUniversityComputationalRobotics/esc-nav-stack/blob/world_modeling/esc_move_base_msgs/action/Goto2D.action)) 281 | 282 | Action to request navigation query. The name of the action server depends on the parameter `goto_action`. 283 | 284 | ## Path Following Control (`esc_move_base_control`) 285 | 286 | This module is in charge of sending the velocities to the robot according to the path obtained so that the robot goes through that path. 287 | 288 | ### Parameters 289 | 290 | - max_vel (double, default: 0.1) 291 | 292 | Maximum velocity the robot can have in the X axis. 293 | 294 | - min_vel (double, default: 0.05) 295 | 296 | Minimum velocity for the robot to move in the X axis. 297 | 298 | - max_turn_rate (double, default: 0.5) 299 | 300 | Maximum velocity with which the robot can rotate. 301 | 302 | - min_turn_rate (double, default: 0.5) 303 | 304 | Minimum velocity for the robot to rotate. 305 | 306 | - controller_hz (double, default: 100) 307 | 308 | Frequency in which velocities are sent 309 | 310 | - control_path_topic (string, default: "/control_path_topic") 311 | 312 | Topic in which the desired path to follow is published. 313 | 314 | - control_output_topic (double, default: "/control_output_topic") 315 | 316 | Topic in which the velocities are published. 317 | 318 | - odometry_topic (string, default: "/odometry_topic") 319 | 320 | ### Subscribers 321 | 322 | The name of the subscribers' topics are just defined as an example, but they may be configured using the parameters defined before. 323 | 324 | - /esc_move_base_planner/esc_move_base_solution_path ([esc_move_base_msgs/Path2D](https://github.com/CardiffUniversityComputationalRobotics/esc-nav-stack/blob/world_modeling/esc_move_base_msgs/msg/Path2D.msg)) 325 | - /pepper/odom_groundtruth ([nav_msgs/Odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html)) 326 | 327 | ### Publishers 328 | 329 | The name of the publishers' topics are just defined as an example, but they may be configured using the parameters defined before. 330 | 331 | - /control_active_topic ([std_msgs/Bool](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Bool.html)) 332 | - /pepper/cmd_vel ([geometry_msgs/Twist](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Twist.html)) 333 | 334 | ## Cite Our Paper 335 | 336 | If this navigation framework is used for research or any other work, please make sure to cite our paper with the following bibtex: 337 | 338 | ``` 339 | @inproceedings{Silva2022, 340 | author={Silva, Steven and Paillacho, Dennys and Verdezoto, Nervo and Hern{\'{a}}ndez, Juan David}, 341 | booktitle={2022 IEEE 18th International Conference on Automation Science and Engineering (CASE)}, 342 | title={Towards Online Socially Acceptable Robot Navigation}, 343 | year={2022}, 344 | pages={707-714}, 345 | doi={10.1109/CASE49997.2022.9926686}, 346 | url = {10.1109/CASE49997.2022.9926686}, 347 | isbn = {9781665490429}, 348 | } 349 | ``` 350 | -------------------------------------------------------------------------------- /esc_move_base_control/esc_move_base_control/base_controller.py: -------------------------------------------------------------------------------- 1 | """ 2 | Created on November 1, 2018 3 | 4 | @author: juandhv (Juan David Hernandez Vega, juandhv@rice.edu) 5 | 6 | Purpose: Alternative esc base controller 7 | """ 8 | 9 | import math 10 | import numpy as np 11 | 12 | import rclpy 13 | from rclpy.node import Node 14 | 15 | # ROS messages 16 | from geometry_msgs.msg import Twist 17 | from std_msgs.msg import Bool 18 | from nav_msgs.msg import Odometry 19 | from esc_move_base_msgs.msg import Path2D 20 | from tf_transformations import euler_from_quaternion 21 | 22 | 23 | class Controller(Node): 24 | """ 25 | Controller class 26 | """ 27 | 28 | def __init__(self): 29 | """ 30 | Constructor 31 | """ 32 | super().__init__("esc_move_base_control") 33 | 34 | # ======================================================================= 35 | # Initial values 36 | # ======================================================================= 37 | self.current_position_ = np.zeros(2) 38 | self.current_orientation_ = 0.0 39 | 40 | self.desired_position_ = np.zeros(2) 41 | self.desired_orientation_ = 0.0 42 | 43 | self.solution_path_wps_ = [] 44 | 45 | # ======================================================================= 46 | # Path trimmer variables 47 | # ======================================================================= 48 | self.yaw = 0 49 | 50 | # ======================================================================= 51 | # Get parameters 52 | # ======================================================================= 53 | self.declare_parameter("max_vel", 0.1) 54 | self.declare_parameter("min_vel", 0.05) 55 | self.declare_parameter("max_turn_rate", 0.5) 56 | self.declare_parameter("min_turn_rate", 0.5) 57 | self.declare_parameter("drift_turning_vel", 0.0) 58 | self.declare_parameter("controller_hz", 100.0) 59 | self.declare_parameter("odometry_topic", "odometry_topic") 60 | self.declare_parameter("control_path_topic", "control_path_topic") 61 | self.declare_parameter("control_output_topic", "control_output_topic") 62 | self.declare_parameter("control_active_topic", "control_active_topic") 63 | self.declare_parameter("xy_goal_tolerance", 0.2) 64 | self.declare_parameter("yaw_goal_tolerance", 0.2) 65 | 66 | self.max_vel_ = self.get_parameter("max_vel").get_parameter_value().double_value 67 | self.min_vel_ = self.get_parameter("min_vel").get_parameter_value().double_value 68 | self.max_turn_rate_ = ( 69 | self.get_parameter("max_turn_rate").get_parameter_value().double_value 70 | ) 71 | self.min_turn_rate_ = ( 72 | self.get_parameter("min_turn_rate").get_parameter_value().double_value 73 | ) 74 | self.drift_turning_vel_ = ( 75 | self.get_parameter("drift_turning_vel").get_parameter_value().double_value 76 | ) 77 | self.controller_hz_ = ( 78 | self.get_parameter("controller_hz").get_parameter_value().double_value 79 | ) 80 | self.odometry_topic_ = ( 81 | self.get_parameter("odometry_topic").get_parameter_value().string_value 82 | ) 83 | self.control_path_topic_ = ( 84 | self.get_parameter("control_path_topic").get_parameter_value().string_value 85 | ) 86 | self.control_output_topic_ = ( 87 | self.get_parameter("control_output_topic") 88 | .get_parameter_value() 89 | .string_value 90 | ) 91 | self.control_active_topic_ = ( 92 | self.get_parameter("control_active_topic") 93 | .get_parameter_value() 94 | .string_value 95 | ) 96 | self.xy_goal_tolerance_ = ( 97 | self.get_parameter("xy_goal_tolerance").get_parameter_value().double_value 98 | ) 99 | self.yaw_goal_tolerance_ = ( 100 | self.get_parameter("yaw_goal_tolerance").get_parameter_value().double_value 101 | ) 102 | 103 | # ======================================================================= 104 | # Subscribers 105 | # ======================================================================= 106 | # Navigation data (feedback) 107 | self.odometry_sub_ = self.create_subscription( 108 | Odometry, self.odometry_topic_, self.odomCallback, 10 109 | ) 110 | self.control_path_sub_ = self.create_subscription( 111 | Path2D, self.control_path_topic_, self.receiveControlPathCallback, 10 112 | ) 113 | 114 | # ======================================================================= 115 | # Publishers 116 | # ======================================================================= 117 | self.control_output_pub_ = self.create_publisher( 118 | Twist, self.control_output_topic_, 10 119 | ) 120 | self.control_active_pub_ = self.create_publisher( 121 | Bool, self.control_active_topic_, 10 122 | ) 123 | self.controller_state = 0 124 | 125 | self.timer = self.create_timer(1 / self.controller_hz_, self.controlBaseEsc) 126 | 127 | def odomCallback(self, odometry_msg): 128 | """ 129 | Callback to robot's odometry data 130 | """ 131 | self.current_position_[0] = odometry_msg.pose.pose.position.x 132 | self.current_position_[1] = odometry_msg.pose.pose.position.y 133 | (r, p, self.yaw) = euler_from_quaternion( 134 | [ 135 | odometry_msg.pose.pose.orientation.x, 136 | odometry_msg.pose.pose.orientation.y, 137 | odometry_msg.pose.pose.orientation.z, 138 | odometry_msg.pose.pose.orientation.w, 139 | ] 140 | ) 141 | self.current_orientation_ = wrapAngle(self.yaw) 142 | return 143 | 144 | def receiveControlPathCallback(self, path_2d_msg): 145 | """Callback to receive path (list of waypoints)""" 146 | self.solution_path_wps_ = [] 147 | 148 | waypoint_distances = np.array([]) 149 | min_list_index = [] 150 | actual_next_waypoint_index = None 151 | 152 | waypoints_list = path_2d_msg.waypoints 153 | # print(waypoints_list) 154 | # print("length:", len(waypoints_list)) 155 | 156 | if len(waypoints_list) <= 3: 157 | self.solution_path_wps_.append( 158 | [waypoints_list[-1].x, waypoints_list[-1].y, waypoints_list[-1].theta] 159 | ) 160 | return 161 | 162 | for waypoint in waypoints_list: 163 | waypoint_distances = np.append( 164 | waypoint_distances, 165 | [ 166 | euclidian_distance( 167 | self.current_position_[0], 168 | self.current_position_[1], 169 | waypoint.x, 170 | waypoint.y, 171 | ) 172 | ], 173 | ) 174 | # print(len(waypoint_distances)) 175 | for i in range(0, 3): 176 | min_list_index.append( 177 | np.where(waypoint_distances == np.amin(waypoint_distances))[0][0] 178 | ) 179 | waypoint_distances[min_list_index[i]] = float("inf") 180 | # print(min_list_index) 181 | current_waypoint_angle = float("inf") 182 | for i in min_list_index: 183 | new_waypoint_angle = self.robotAngleToPoint( 184 | path_2d_msg.waypoints[i].x, path_2d_msg.waypoints[i].y 185 | ) 186 | if new_waypoint_angle < current_waypoint_angle: 187 | actual_next_waypoint_index = i 188 | current_waypoint_angle = new_waypoint_angle 189 | 190 | if actual_next_waypoint_index is not None: 191 | waypoints_list = waypoints_list[actual_next_waypoint_index + 1 :] 192 | 193 | for i in range(1, len(waypoints_list)): 194 | waypoint = waypoints_list[i] 195 | distance_to_wp = math.sqrt( 196 | math.pow(waypoint.x - self.current_position_[0], 2.0) 197 | + math.pow(waypoint.y - self.current_position_[1], 2.0) 198 | ) 199 | if distance_to_wp > self.xy_goal_tolerance_ or i == ( 200 | len(waypoints_list) - 1 201 | ): 202 | self.solution_path_wps_.append([waypoint.x, waypoint.y, waypoint.theta]) 203 | return 204 | 205 | def robotAngleToPoint(self, x, y): 206 | 207 | tetha_robot_waypoint = np.arctan2( 208 | y - self.current_position_[1], x - self.current_position_[0] 209 | ) 210 | 211 | if tetha_robot_waypoint < 0: 212 | tetha_robot_waypoint = 2 * math.pi + tetha_robot_waypoint 213 | 214 | robotAngle = self.yaw 215 | 216 | if robotAngle < 0: 217 | robotAngle = 2 * math.pi + robotAngle 218 | 219 | if tetha_robot_waypoint > (robotAngle + math.pi): 220 | tetha_robot_waypoint = abs(robotAngle + 2 * math.pi - tetha_robot_waypoint) 221 | elif robotAngle > (tetha_robot_waypoint + math.pi): 222 | tetha_robot_waypoint = abs(tetha_robot_waypoint + 2 * math.pi - robotAngle) 223 | else: 224 | tetha_robot_waypoint = abs(tetha_robot_waypoint - robotAngle) 225 | 226 | return tetha_robot_waypoint 227 | 228 | def controlBaseEsc(self): 229 | """Control loop""" 230 | # print(self.solution_path_wps_) 231 | if len(self.solution_path_wps_) > 0: 232 | # print(self.solution_path_wps_) 233 | self.desired_position_[0] = self.solution_path_wps_[0][0] 234 | self.desired_position_[1] = self.solution_path_wps_[0][1] 235 | inc_x = self.desired_position_[0] - self.current_position_[0] 236 | inc_y = self.desired_position_[1] - self.current_position_[1] 237 | distance_to_goal = math.sqrt(math.pow(inc_x, 2.0) + math.pow(inc_y, 2.0)) 238 | control_input = Twist() 239 | control_input.angular.x = 0.0 240 | control_input.angular.y = 0.0 241 | control_input.angular.z = 0.0 242 | control_input.linear.x = 0.0 243 | control_input.linear.y = 0.0 244 | control_input.linear.z = 0.0 245 | 246 | if distance_to_goal >= 0.4: 247 | self.controller_state = 0 248 | self.desired_orientation_ = wrapAngle(math.atan2(inc_y, inc_x)) 249 | yaw_error = wrapAngle( 250 | self.desired_orientation_ - self.current_orientation_ 251 | ) 252 | 253 | if abs(yaw_error) > 0.2: 254 | self.get_logger().debug( 255 | "orienting towards the next waypoint: " + str(yaw_error), 256 | ) 257 | control_input.angular.z = yaw_error * self.max_turn_rate_ 258 | else: 259 | self.get_logger().debug( 260 | "moving towards the next waypoint: " + str(yaw_error) 261 | ) 262 | control_input.angular.z = yaw_error * self.max_turn_rate_ 263 | 264 | liner_speed = abs(distance_to_goal) * 0.5 265 | if liner_speed < self.min_vel_: 266 | control_input.linear.x = self.min_vel_ 267 | elif liner_speed > self.max_vel_: 268 | control_input.linear.x = self.max_vel_ 269 | else: 270 | control_input.linear.x = liner_speed 271 | 272 | if control_input.angular.z < -self.max_turn_rate_: 273 | control_input.angular.z = -self.max_turn_rate_ 274 | elif control_input.angular.z > self.max_turn_rate_: 275 | control_input.angular.z = self.max_turn_rate_ 276 | self.control_output_pub_.publish(control_input) 277 | 278 | else: 279 | if len(self.solution_path_wps_) > 1: 280 | del self.solution_path_wps_[0] 281 | else: 282 | if ( 283 | distance_to_goal >= self.xy_goal_tolerance_ 284 | and self.controller_state != 2 285 | ): 286 | self.controller_state = 1 287 | self.desired_orientation_ = wrapAngle(math.atan2(inc_y, inc_x)) 288 | yaw_error = wrapAngle( 289 | self.desired_orientation_ - self.current_orientation_ 290 | ) 291 | if abs(yaw_error) > 0.2 and abs(yaw_error) < 1.57: 292 | self.get_logger().debug( 293 | "final approach: orienting towards a waypoint (forward)" 294 | ) 295 | if yaw_error > 0.0: 296 | control_input.angular.z = self.min_turn_rate_ 297 | else: 298 | control_input.angular.z = -self.min_turn_rate_ 299 | control_input.linear.x = self.drift_turning_vel_ 300 | elif abs(yaw_error) > 1.57 and abs(yaw_error) < 2.94: 301 | self.get_logger().debug( 302 | "final approach: orienting towards a waypoint (backward)", 303 | ) 304 | if yaw_error > 0.0: 305 | control_input.angular.z = -self.min_turn_rate_ 306 | else: 307 | control_input.angular.z = self.min_turn_rate_ 308 | 309 | control_input.linear.x = -self.drift_turning_vel_ 310 | elif abs(yaw_error) < 0.2: 311 | self.get_logger().debug( 312 | "final approach: moving towards a waypoint (forward)" 313 | ) 314 | 315 | control_input.linear.x = 0.05 316 | else: 317 | self.get_logger().debug( 318 | "final approach: moving towards a waypoint (backward)", 319 | ) 320 | 321 | control_input.linear.x = -0.05 322 | 323 | self.control_output_pub_.publish(control_input) 324 | else: 325 | self.controller_state = 2 326 | self.get_logger().debug( 327 | "final approach, final orientation", 328 | ) 329 | yaw_error = wrapAngle( 330 | self.solution_path_wps_[0][2] - self.current_orientation_ 331 | ) 332 | 333 | if abs(yaw_error) < self.yaw_goal_tolerance_: 334 | if len(self.solution_path_wps_) > 0: 335 | del self.solution_path_wps_[0] 336 | else: 337 | control_input.angular.z = yaw_error * self.max_turn_rate_ 338 | if yaw_error < 0.0: 339 | control_input.linear.x = self.drift_turning_vel_ 340 | control_input.angular.z = -self.min_turn_rate_ * 5 341 | else: 342 | control_input.linear.x = -self.drift_turning_vel_ 343 | control_input.angular.z = self.min_turn_rate_ * 5 344 | self.control_output_pub_.publish(control_input) 345 | 346 | # self.get_logger().debug("%s: yaw_error: %f", self.get_name(), yaw_error) 347 | # self.get_logger().debug("%s: distance_to_goal: %f", self.get_name(), distance_to_goal) 348 | # self.get_logger().debug("%s: control_input.linear.x %f", self.get_name(), control_input.linear.x) 349 | # self.get_logger().debug("%s: control_input.angular.z %f\n", self.get_name(), control_input.angular.z) 350 | self.control_active_pub_.publish(Bool(data=True)) 351 | else: 352 | control_input = Twist() 353 | self.control_output_pub_.publish(control_input) 354 | self.control_active_pub_.publish(Bool(data=False)) 355 | return 356 | 357 | 358 | def euclidian_distance(x1, y1, x2, y2): 359 | return np.sqrt(np.power(x2 - x1, 2) + np.power(y2 - y1, 2)) 360 | 361 | 362 | def wrapAngle(angle): 363 | """wrapAngle 364 | Calculates angles values between 0 and 2pi""" 365 | return angle + (2.0 * math.pi * math.floor((math.pi - angle) / (2.0 * math.pi))) 366 | 367 | 368 | def main(args=None): 369 | rclpy.init(args=args) 370 | controller_node = Controller() 371 | rclpy.spin(controller_node) 372 | controller_node.destroy_node() 373 | rclpy.shutdown() 374 | 375 | 376 | if __name__ == "__main__": 377 | main() 378 | -------------------------------------------------------------------------------- /esc_move_base_planning/include/planner/RRTstarMod.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Rice University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Rice University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Authors: Alejandro Perez, Sertac Karaman, Ryan Luna, Luis G. Torres, Ioan Sucan, Javier V Gomez, Jonathan 36 | * Gammell */ 37 | 38 | /* Modified by: Juan D. Hernandez */ 39 | 40 | #ifndef OMPL_GEOMETRIC_PLANNERS_RRT_RRTSTAR_MOD_ 41 | #define OMPL_GEOMETRIC_PLANNERS_RRT_RRTSTAR_MOD_ 42 | 43 | #include "ompl/geometric/planners/PlannerIncludes.h" 44 | #include "ompl/base/OptimizationObjective.h" 45 | #include "ompl/datastructures/NearestNeighbors.h" 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | namespace ompl 55 | { 56 | namespace geometric 57 | { 58 | /** 59 | @anchor gRRTstar 60 | @par Short description 61 | \ref gRRTstar "RRT*" (optimal RRT) is an asymptotically-optimal incremental 62 | sampling-based motion planning algorithm. \ref gRRTstar "RRT*" algorithm is 63 | guaranteed to converge to an optimal solution, while its 64 | running time is guaranteed to be a constant factor of the 65 | running time of the \ref gRRT "RRT". The notion of optimality is with 66 | respect to the distance function defined on the state space 67 | we are operating on. See ompl::base::Goal::setMaximumPathLength() for 68 | how to set the maximally allowed path length to reach the goal. 69 | If a solution path that is shorter than ompl::base::Goal::getMaximumPathLength() is 70 | found, the algorithm terminates before the elapsed time. 71 | @par External documentation 72 | S. Karaman and E. Frazzoli, Sampling-based 73 | Algorithms for Optimal Motion Planning, International Journal of Robotics 74 | Research, Vol 30, No 7, 2011. 75 | http://arxiv.org/abs/1105.1186 76 | */ 77 | 78 | /** \brief Optimal Rapidly-exploring Random Trees */ 79 | class RRTstarMod : public base::Planner 80 | { 81 | public: 82 | RRTstarMod(const base::SpaceInformationPtr &si); 83 | 84 | virtual ~RRTstarMod(); 85 | 86 | virtual void getPlannerData(base::PlannerData &data) const; 87 | 88 | virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc); 89 | 90 | virtual void clear(); 91 | 92 | virtual void setup(); 93 | 94 | /** \brief Set the goal bias 95 | 96 | In the process of randomly selecting states in 97 | the state space to attempt to go towards, the 98 | algorithm may in fact choose the actual goal state, if 99 | it knows it, with some probability. This probability 100 | is a real number between 0.0 and 1.0; its value should 101 | usually be around 0.05 and should not be too large. It 102 | is probably a good idea to use the default value. */ 103 | void setGoalBias(double goalBias) 104 | { 105 | goalBias_ = goalBias; 106 | } 107 | 108 | /** \brief Get the goal bias the planner is using */ 109 | double getGoalBias() const 110 | { 111 | return goalBias_; 112 | } 113 | 114 | /** \brief Set the range the planner is supposed to use. 115 | 116 | This parameter greatly influences the runtime of the 117 | algorithm. It represents the maximum length of a 118 | motion to be added in the tree of motions. */ 119 | void setRange(double distance) 120 | { 121 | maxDistance_ = distance; 122 | } 123 | 124 | /** \brief Get the range the planner is using */ 125 | double getRange() const 126 | { 127 | return maxDistance_; 128 | } 129 | 130 | /** \brief Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg* (or k_rrg = s 131 | * \times k_rrg*) */ 132 | void setRewireFactor(double rewireFactor) 133 | { 134 | rewireFactor_ = rewireFactor; 135 | calculateRewiringLowerBounds(); 136 | } 137 | 138 | /** \brief Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg* > r_rrg* (or k_rrg 139 | * = s \times k_rrg* > k_rrg*) */ 140 | double getRewireFactor() const 141 | { 142 | return rewireFactor_; 143 | } 144 | 145 | /** \brief Set a different nearest neighbors datastructure */ 146 | template