├── CMakeLists.txt ├── LICENSE ├── README.md ├── action └── Explore.action ├── cfg └── Planner.cfg ├── include ├── ExplorationPlanner │ ├── grid │ │ ├── IPlanarGridMap.h │ │ ├── PlanarGridBinaryMap.h │ │ ├── PlanarGridContainer.h │ │ ├── PlanarGridIndex.h │ │ └── PlanarGridOccupancyMap.h │ ├── kinematics │ │ ├── IConstraint.h │ │ ├── IPlanarKinematics.h │ │ ├── IPlanarRobotVelCmdSampler.h │ │ ├── PlanarPose.h │ │ ├── PlanarRobotVelCmd.h │ │ ├── PlanarRobotVelCmdSamplerIndependentGaussian.h │ │ ├── PlanarRobotVelCmdSamplerUniform.h │ │ ├── RangeConstraint.h │ │ └── VelocityPlanarKinematics.h │ ├── planner │ │ ├── ExplorationTaskState.h │ │ ├── ICoolingSchedule.h │ │ ├── IPlanningAlgorithm.h │ │ ├── LinearCoolingSchedule.h │ │ ├── Particle.h │ │ ├── ParticleSet.h │ │ ├── SDMPlanner.h │ │ ├── SMCPlanner.h │ │ └── SMCPlannerParameters.h │ ├── sensor │ │ ├── Bresenham2D.h │ │ ├── LaserScanner2D.h │ │ ├── PlanarCellObservation.h │ │ ├── PlanarObservation.h │ │ └── Raytracer2D.h │ ├── trajectory │ │ ├── TrajectoryChecker.h │ │ ├── TrajectoryEvaluator.h │ │ ├── TrajectoryGenerator.h │ │ ├── TrajectoryValue.h │ │ └── TrajectoryValueHandler.h │ └── utils │ │ ├── EntropyLUT.h │ │ ├── LogOddsLUT.h │ │ └── RNG.h └── ExplorationPlannerROS │ └── ExplorationPlannerROS.h ├── launch ├── move_base.launch.xml ├── simulator_exploration.launch └── turtlebot_simulator.launch ├── maps ├── mit.png ├── mit.yaml └── stage │ ├── mit.world │ └── turtlebot.inc ├── package.xml ├── param ├── frontier_exploration.yaml ├── navigation │ ├── costmap_common_params.yaml │ ├── dwa_local_planner_params.yaml │ ├── global_costmap_params.yaml │ ├── global_planner_params.yaml │ ├── local_costmap_params.yaml │ ├── move_base_params.yaml │ └── navfn_global_planner_params.yaml └── planner.yaml ├── rviz └── simulator_exploration.rviz └── src ├── ExplorationPlanner ├── grid │ ├── PlanarGridBinaryMap.cpp │ └── PlanarGridOccupancyMap.cpp ├── kinematics │ ├── PlanarPose.cpp │ ├── PlanarRobotVelCmd.cpp │ ├── PlanarRobotVelCmdSamplerIndependentGaussian.cpp │ ├── PlanarRobotVelCmdSamplerUniform.cpp │ ├── RangeConstraint.cpp │ └── VelocityPlanarKinematics.cpp ├── planner │ ├── ExplorationTaskState.cpp │ ├── ICoolingSchedule.cpp │ ├── Particle.cpp │ ├── ParticleSet.cpp │ ├── SMCPlanner.cpp │ └── SMCPlannerParameters.cpp ├── sensor │ ├── Bresenham2D.cpp │ ├── LaserScanner2D.cpp │ └── Raytracer2D.cpp ├── trajectory │ ├── TrajectoryChecker.cpp │ ├── TrajectoryEvaluator.cpp │ ├── TrajectoryGenerator.cpp │ ├── TrajectoryValue.cpp │ └── TrajectoryValueHandler.cpp └── utils │ ├── EntropyLUT.cpp │ └── LogOddsLUT.cpp ├── ExplorationPlannerROS └── ExplorationPlannerROS.cpp └── exploration_planner_node.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ase_exploration) 3 | 4 | ## Find catkin macros and libraries 5 | find_package(catkin REQUIRED COMPONENTS 6 | geometry_msgs 7 | nav_msgs 8 | map_msgs 9 | roscpp 10 | tf 11 | move_base 12 | actionlib 13 | actionlib_msgs 14 | message_generation 15 | dynamic_reconfigure 16 | frontier_exploration 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | # Use OpenMP 23 | OPTION (USE_OpenMP "Use OpenMP" ON) 24 | IF(USE_OpenMP) 25 | FIND_PACKAGE(OpenMP) 26 | IF(OPENMP_FOUND) 27 | SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 28 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 29 | ENDIF() 30 | ENDIF() 31 | 32 | # check c++11 / c++0x 33 | include(CheckCXXCompilerFlag) 34 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 35 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 36 | if(COMPILER_SUPPORTS_CXX11) 37 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 38 | elseif(COMPILER_SUPPORTS_CXX0X) 39 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 40 | else() 41 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 42 | endif() 43 | 44 | ## Generate actions in the 'action' folder 45 | add_action_files( 46 | FILES 47 | Explore.action 48 | ) 49 | 50 | ## Generate added messages and services with any dependencies listed here 51 | generate_messages( 52 | DEPENDENCIES 53 | geometry_msgs 54 | std_msgs 55 | actionlib_msgs 56 | ) 57 | 58 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 59 | generate_dynamic_reconfigure_options( 60 | cfg/Planner.cfg 61 | ) 62 | 63 | ################################### 64 | ## catkin specific configuration ## 65 | ################################### 66 | ## The catkin_package macro generates cmake config files for your package 67 | ## Declare things to be passed to dependent projects 68 | ## INCLUDE_DIRS: uncomment this if you package contains header files 69 | ## LIBRARIES: libraries you create in this project that dependent projects also need 70 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 71 | ## DEPENDS: system dependencies of this project that dependent projects also need 72 | catkin_package( 73 | INCLUDE_DIRS include 74 | ) 75 | 76 | ########### 77 | ## Build ## 78 | ########### 79 | 80 | ## Specify additional locations of header files 81 | ## Your package locations should be listed before other locations 82 | include_directories( 83 | include 84 | ${catkin_INCLUDE_DIRS} 85 | ) 86 | 87 | ## Declare a C++ library 88 | # ROS interface for planner 89 | add_library(exploration_planner_ros 90 | src/ExplorationPlannerROS/ExplorationPlannerROS.cpp 91 | ) 92 | 93 | ## ExplorationPlanner related libraries 94 | add_library(ase_exploration_grid 95 | src/ExplorationPlanner/grid/PlanarGridOccupancyMap.cpp 96 | src/ExplorationPlanner/grid/PlanarGridBinaryMap.cpp 97 | ) 98 | 99 | add_library(ase_exploration_trajectory 100 | src/ExplorationPlanner/trajectory/TrajectoryChecker.cpp 101 | src/ExplorationPlanner/trajectory/TrajectoryEvaluator.cpp 102 | src/ExplorationPlanner/trajectory/TrajectoryGenerator.cpp 103 | src/ExplorationPlanner/trajectory/TrajectoryValue.cpp 104 | src/ExplorationPlanner/trajectory/TrajectoryValueHandler.cpp 105 | ) 106 | 107 | add_library(ase_exploration_planner 108 | src/ExplorationPlanner/planner/ExplorationTaskState.cpp 109 | src/ExplorationPlanner/planner/ICoolingSchedule.cpp 110 | src/ExplorationPlanner/planner/Particle.cpp 111 | src/ExplorationPlanner/planner/ParticleSet.cpp 112 | src/ExplorationPlanner/planner/SMCPlanner.cpp 113 | src/ExplorationPlanner/planner/SMCPlannerParameters.cpp 114 | src/ExplorationPlanner/kinematics/RangeConstraint.cpp 115 | src/ExplorationPlanner/kinematics/VelocityPlanarKinematics.cpp 116 | src/ExplorationPlanner/kinematics/PlanarPose.cpp 117 | src/ExplorationPlanner/kinematics/PlanarRobotVelCmd.cpp 118 | src/ExplorationPlanner/kinematics/PlanarRobotVelCmdSamplerIndependentGaussian.cpp 119 | src/ExplorationPlanner/kinematics/PlanarRobotVelCmdSamplerUniform.cpp 120 | ) 121 | 122 | add_library(ase_exploration_sensor 123 | src/ExplorationPlanner/sensor/Bresenham2D.cpp 124 | src/ExplorationPlanner/sensor/Raytracer2D.cpp 125 | src/ExplorationPlanner/sensor/LaserScanner2D.cpp 126 | ) 127 | 128 | add_library(ase_exploration_lookup_tables 129 | src/ExplorationPlanner/utils/EntropyLUT.cpp 130 | src/ExplorationPlanner/utils/LogOddsLUT.cpp 131 | ) 132 | 133 | set(ASE_EXPLORATION_PLANNER_LIBS 134 | ase_exploration_grid 135 | ase_exploration_trajectory 136 | ase_exploration_planner 137 | ase_exploration_sensor 138 | ase_exploration_lookup_tables 139 | ) 140 | 141 | 142 | # Exploration planner ROS needs the messages generated 143 | add_dependencies(exploration_planner_ros ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 144 | 145 | ## Declare a C++ executable 146 | # Exploration planner 147 | add_executable(${PROJECT_NAME}_planner_node src/exploration_planner_node.cpp) 148 | 149 | ## Specify libraries to link a library or executable target against 150 | target_link_libraries(exploration_planner_ros 151 | ${catkin_LIBRARIES} 152 | ) 153 | 154 | target_link_libraries(ase_exploration_planner 155 | ase_exploration_trajectory 156 | ) 157 | 158 | target_link_libraries(${PROJECT_NAME}_planner_node 159 | ${catkin_LIBRARIES} 160 | exploration_planner_ros 161 | ${ASE_EXPLORATION_PLANNER_LIBS} 162 | ) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016, Mikko Lauri 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | 6 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | 8 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | 10 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | 12 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Forward-simulation based planning for robotic exploration 2 | The software contained in the ase_exploration package is a [ROS](www.ros.org) integrated planner for robotic exploration tasks, taking as input data from various costmaps and outputting new exploration targets for the robot. 3 | To execute the exploration task, this software uses [move_base](http://wiki.ros.org/move_base). 4 | In order to use this software, you will need to have properly configured move_base to run on your robot. 5 | 6 | The planner software uses layers of costmaps to track free, occupied, and unknown space. 7 | Based on this information, it will sample possible trajectories and sensor readings, and iteratively improve the trajectories to maximize the mutual information of the map and future observations. 8 | Based on a few heuristic rules, the planner detects whether the robot might be stuck or if the local trajectories it has sampled are not very informative. 9 | If this happens, the planner will instead apply classical frontier-based exploration to select the next exploration target, and afterwards again resumes planning via forward simulation of local trajectories. 10 | For technical details on the approach, please refer to the publication listed below. 11 | 12 | The basic interface is through ROS actions - as long as an request for exploration provided in the form of an action is active, the planner will keep producing new exploration targets. 13 | The process is stopped when an error occurs, or when the user cancels the action. 14 | The software will pass any exploration targets it computes to move_base via a move_base_msgs::MoveBaseAction type action. 15 | 16 | # Video and publication 17 | 18 | Click the link below to view a video showing the software in action: 19 | [![Planning for robotic exploration based on forward simulation](http://i.imgur.com/AuwCkYr.png)](https://www.youtube.com/watch?v=-7c_RaZ-KTU) 20 | 21 | Further details are available in our journal paper on the subject. 22 | If you use this code in an academic context, please cite the paper: 23 | 24 | Mikko Lauri, Risto Ritala. *Planning for robotic exploration based on forward simulation*, Robotics and Autonomous Systems, Vol 83, (2016), pp. 15-31, DOI: [10.1016/j.robot.2016.06.008](https://doi.org/10.1016/j.robot.2016.06.008) 25 | 26 | A preprint version is also [available on arXiv](https://arxiv.org/abs/1502.02474). 27 | 28 | BiBTeX: 29 | ``` 30 | @article{Lauri2016planning, 31 | author = {Mikko Lauri and Risto Ritala}, 32 | title = {Planning for robotic exploration based on forward simulation}, 33 | journal = {Robotics and Autonomous Systems}, 34 | year = 2016, 35 | volume = 83, 36 | pages = {15 - 31}, 37 | doi = {10.1016/j.robot.2016.06.008} 38 | } 39 | ``` 40 | 41 | ### Acknowledgment 42 | As implementation of the fallback exploration strategy based on classical frontier exploration, we apply Paul Bovbel's [frontier_exploration](https://github.com/paulbovbel/frontier_exploration) package. 43 | This package also in part inspired the implementation our planner. 44 | 45 | # Installation 46 | The code is hosted on [GitHub](https://github.com/laurimi/ase_exploration). 47 | The package has been tested to work with ROS Indigo, running on Ubuntu 14.04, using gcc 4.8.4 as compiler. 48 | To build, simply clone this repository to your catkin workspace `src` directory, and run `catkin_make` in the workspace main directory. 49 | 50 | For improved performance, this package uses OpenMP for evaluating trajectories in parallel. 51 | C++11 features are used in the code, and support is required from the compiler. 52 | 53 | 54 | # Simulator demo: Turtlebot exploring an environment 55 | You can test the exploration planner by running it using simulator. 56 | Here, we apply mostly standard settings from the [turtlebot_stage](http://wiki.ros.org/turtlebot_stage) package. 57 | To run the demo, you need to install the dependencies: 58 | ``` 59 | sudo apt-get install ros-indigo-turtlebot-simulator ros-indigo-frontier-exploration ros-indigo-turtlebot-navigation 60 | ``` 61 | 62 | Before continuing, make sure you have sourced `setup.bash` of the catkin workspace you installed the exploration package in, do this as follows (changing the path appropriately) 63 | ``` 64 | source ~/catkin_ws/devel/setup.bash 65 | ``` 66 | 67 | Now run the main launch file: 68 | ``` 69 | roslaunch ase_exploration simulator_exploration.launch 70 | ``` 71 | The simulator will now start, along with other nodes and an rviz visualization. 72 | 73 | Finally, start the action client to send an exploration command to the robot. 74 | **This command must be run in a terminal window where `setup.bash` of the catkin workspace you installed the exploration package in has been sourced.** 75 | ``` 76 | rosrun actionlib axclient.py /exploration 77 | ``` 78 | Click on "Send", and the simulated Turtlebot will start exploring the environment. 79 | 80 | # ROS Node details: ase_exploration_planner_node 81 | 82 | ## 1. Actions provided 83 | This node provides an implementation of the SimpleActionServer for the ExploreAction type defined in this package. 84 | This action is provided with the name `exploration`. 85 | Your best bet to get started is probably to use actionlib's axclient to send an action request to start exploration: `rosrun actionlib axclient.py /exploration`. 86 | 87 | ### ExploreAction 88 | ExploreAction is very simple. It has an empty goal definition, and an empty result definition, and as feedback will output the current exploration target produced by the planner. 89 | Once the user gives the node a exploration task through this action, the planner will produce exploration targets until an error occurs or the user cancels the action. 90 | 91 | ## 2. Actions called 92 | The exploration targets produced by the planner are sent as navigation goals to move_base. 93 | This is handled by a SimpleActionClient calling an move_base_msgs::MoveBaseAction on the topic `move_base`. 94 | 95 | ## 3. Services required 96 | This node requires and calls two services from the [frontier_exploration](https://github.com/paulbovbel/frontier_exploration) package. 97 | They are: 98 | 99 | - `frontier_exploration/UpdateBoundaryPolygon` 100 | - `frontier_exploration/GetNextFrontier` 101 | 102 | In practice you will need to launch an instance of `frontier_exploration` and set its parameters according to the behaviour you want in case this exploration node has to fallback to using frontier exploration. 103 | The `ase_exploration_node` will call services from `frontier_exploration` as required. 104 | 105 | ## 4. Subscribed topics 106 | The node requires two types of maps as input in order to work: a costmap describing where the robot is allowed to and not allowed to plan paths, and a global map describing the current information the robot has about the environment. 107 | For the cost map, we need to subscribe both to the costmap itself and any possible updates to it, as described in the following. 108 | 109 | - `costmap` - a nav_msgs::OccupancyGrid type of costmap describing where the robot is allowed to plan paths to. All values less than or equal to 98 are interpreted as allowed for planning. For controlling behaviour in unknown areas, see the parameter `allow_unknown_targets`. 110 | - `costmap_updates` - a map_msgs::OccupancyGridUpdates message containing updates to partial areas in the global costmap. 111 | - `map` a nav_msgs::OccupancyGrid type of map message containing a global map. This can be produced e.g. by a SLAM algorithm. This map is used at each planning stage as starting information, and should be updated to reflect exploration progress. 112 | 113 | 114 | ## 5. Published topics 115 | - `planner_paths` a nav_msgs::Path type message where all paths the planner has considered on each iteration are published. Note that to visualize these e.g. in rviz, you should set the number of paths displayed equal to the number of particles defined by the parameter `num_particles`. 116 | 117 | ## 6. Parameters 118 | These parameters can be set at node startup. 119 | The planner behaviour control parameters set the thresholds for when the planner will think that the robot is stuck or that local trajectories are not informative. 120 | In case these thresholds are triggered, the planner will fall back to a frontier exploration for selecting the next target and then resume planning via local trajectories. 121 | 122 | ### Frame ids 123 | - `map_frame_id` the map frame in which planning will be done. This must match the incoming "map", "costmap", and "costmap_updates" topics' frame_id. (string, default: map) 124 | - `base_frame_id` base frame of the robot. (string, default: base_link) 125 | 126 | A TF transform must be available between `map_frame_id` and `base_frame_id`. 127 | 128 | ### Planner behaviour control 129 | - `goal_execution_time` the maximum amount of time in seconds that a single exploration target will be active. Once the robot reaches the target or this limit is exceeded, it will trigger a new planning phase. (double, default: 10.0) 130 | - `min_traj_length` minimum length of trajectories in meters for the planner to consider them valid (double, default: 0.2) 131 | - `min_reward` minimum amount of reward units obtained for trajectories for the planner to consider them valid. Roughly speaking, this value corresponds to the the required expected number of unknown cells in the map that will be observed as either free or occupied if the robot were to traverse the trajectory. (double, default: 30.0) 132 | - `max_sampling_tries` maximum number of samples to try to draw for each trajectory before giving up and declaring that no valid trajectory could be found. (int, default: 30) 133 | - `frontier_distance_threshold` if the exploration target was obtained via frontier exploration, the minimum distance in meters when the robot is considered to have reached this target and planning via local trajectories is resumed. (double, default: 2.0) 134 | - `wait_between_planner_iterations` controls whether to wait for user input (keypress) between iterations of the planning algorithm. Useful for debugging and inspecting the evolution of the trajectories considered by the planner during each iteration. (bool, default: False) 135 | 136 | ### Robot dynamics model limits 137 | Only trajectories respecting these limits will be sampled by the planner. 138 | 139 | - `lin_v_min` absolute minimum linear velocity for the robot in meters per second (double, default: 0.3) 140 | - `lin_v_max` absolute maximum linear velocity for the robot in meters per second (double, default: 1.0) 141 | - `ang_v_min` absolute minimum rotational velocity for the robot in radians per second (double, default: -1.0) 142 | - `ang_v_max` absolute maximum rotational velocity for the robot in radians per second (double, default: 1.0) 143 | - `f_rot_min` minimum final rotation at the end of the trajectory of the robot in radians (double, default: -0.02) 144 | - `f_rot_max` maximum final rotation at the end of the trajectory of the robot in radians (double, default: 0.02) 145 | 146 | ## 7. Parameters (dynamically reconfigurable) 147 | From the following parameters, the most important ones to set to correspond to your system are those controlling the simulated laser range finder. 148 | The parameters listed below can also be reconfigured dynamically during runtime using [dynamic_reconfigure](http://wiki.ros.org/dynamic_reconfigure). 149 | 150 | ### Planning algorithm settings 151 | - `horizon` determines how many control actions each trajectory will be composed of. (int, default: 3) 152 | - `discount` determines the relative value of immediate and later rewards, should be greater than 0.0. The smaller this value is, the more emphasis is on immediate information gain. Value 1.0 corresponds to no preference between immediate and later information gain. (double, default: 1.0) 153 | - `schedule_a` at iteration `i`, the planner will use `k = a*i + b` samples to evaluate each trajectory. This sets the parameter a. (int, default: 4) 154 | - `schedule_b` at iteration `i`, the planner will use `k = a*i + b` samples to evaluate each trajectory. This sets the parameter b. (int, default: 3) 155 | - `num_kernels` specifies the total number of iterations the planner will run for. At the first iteration, trajectories are sampled from a uniform distribution, and at later iterations from Gaussian kernels centred at the previous evaluated trajectories. (int, default: 5) 156 | - `std_vel` (double, default: 0.2), `std_ang` (double, default: 0.1), `std_fr` (double, default: 0.02). These parameters control the trajectory sampling process by defining the standard deviations of the Gaussian kernels for the linear velocity (`std_vel`), angular velocity (`std_ang`) and a final rotation at the end of the trajectory (`std_fr`). Velocity is measured in meters per second, angles in radians. At each iteration, the Gaussian kernel used to modify each trajectory samples from three independent Gaussian distributions. The standard deviations for these kernels decrease as function of the iteration. At iteration `i`, the standard deviation applied will be `std_vel / (i+1)`, and so on. This is necessary so the planner does not "lose" good trajectories by modifying them too much at later iterations. 157 | - `num_particles` the number of trajectories maintained over the set of iterations. The larger the number, the more likely it is to converge to a good trajectory. Increasing this parameter will also increase the computational demands the most. (int, default: 10) 158 | - `resample_thresh` the threshold (between 0.0 and 1.0) for the effective number of particles below which resampling will be triggered. (double, default: 0.33) 159 | 160 | 161 | ### Planning settings and constraints 162 | - `allow_unknown_targets` controls whether the planner is allowed to produce exploration targets that are not in free space. Setting this false will make exploration more conservative, as only areas known to be free will be set as exploration goals. (bool, default: True) 163 | - `default_ctrl_duration` the duration of each control action in seconds. Multiplied by `horizon`, this determines the duration of the trajectories considered by the planner. (double, default: 1.0) 164 | 165 | ### Simulated laser scanner parameters 166 | The node samples observations via raytracing with a simulated laser range finder assuming a map hypothesis consistent with the current costmaps. These observation samples are applied to evaluate the trajectories. The simulated laser parameters can be controlled through these parameters. 167 | 168 | - `laser_min_angle_deg` the smallest incidence angle of the laser beam in degrees. (double, default: -90) 169 | - `laser_max_angle_deg` the largest incidence angle of the laser beam in degrees. (double, default: 90) 170 | - `laser_angle_step_deg` the step in degrees between each laser ray. For example, using the default parameters will produce a ray in incidence angles -90, -85, ..., 85, 90. (double, default: 5.0) 171 | - `laser_max_dist_m` the maximum measurable distance for the laser range finder in meters. (double, default: 4.0) 172 | - `laser_p_false_pos` probability of false positive reading for the laser (free cell observed as occupied) (double, default: 0.05) 173 | - `laser_p_false_neg` probability of false negative reading for the laser (occupied cell observed as free)(double, default: 0.05) 174 | 175 | 176 | ## 8. Tips on improving computational performance 177 | The planning method is computationally intensive. 178 | To improve computation speed, you can primarily try to to decrease the length of trajectories by lowering `horizon`, decreasing the number of particles `num_particles`, or the number of samples used in evaluation via `schedule_a` and `schedule_b`, or by decreasing the resolution of the simulated laser by lowering the maximum range `laser_max_dist_m` or making the resolution `laser_angle_step` larger. 179 | 180 | ## 9. Limitations 181 | The software has been written assuming a robot in a planar environment, specifically a Turtlebot-like robot platform. 182 | Possible robot trajectories are sampled under this assumption. 183 | If your robot is not even barely correspond to these assumptions, the dynamics model should be replaced by one suitable for your robot. 184 | 185 | The software currently works on 2D occupancy grid maps and costmaps. 186 | Although there are no plans to extend the software beyond this I have attempted to write the code to enable extensions. 187 | 188 | -------------------------------------------------------------------------------- /action/Explore.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | --- 3 | #result definition 4 | --- 5 | #feedback 6 | geometry_msgs/PoseStamped current_target -------------------------------------------------------------------------------- /cfg/Planner.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "ase_exploration" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("horizon", int_t, 0, "planner horizon", 3, 1, 100) 9 | gen.add("discount", double_t, 0, "planner discount factor", 1.0, 0.0, 1.0) 10 | gen.add("schedule_a", int_t, 0, "a in linear schedule x = a*iter + b for repetitions of SMC", 4, 1, 100) 11 | gen.add("schedule_b", int_t, 0, "b in linear schedule x = a*iter + b for repetitions of SMC", 3, 1, 100) 12 | gen.add("num_kernels", int_t, 0, "number of kernels for SMC", 5, 1, 100) 13 | gen.add("std_vel", double_t, 0, "initial kernel st dev of linear velocity for SMC", 0.2, 0.01, 1) 14 | gen.add("std_ang", double_t, 0, "initial kernel st dev of angular velocity for SMC", 0.1, 0.01, 1) 15 | gen.add("std_fr", double_t, 0, "initial kernel st dev of final rotation for SMC", 0.02, 0.01, 1) 16 | gen.add("default_ctrl_duration", double_t, 0, "default control action duration in seconds", 1.0, 0.1, 5.0) 17 | gen.add("num_particles", int_t, 0, "number of particles in SMC", 10, 1, 500) 18 | gen.add("resample_thresh", double_t, 0, "resampling threshold in SMC", 0.33, 0.0, 1.0) 19 | gen.add("allow_unknown_targets", bool_t, 0, "allow planning targets to unknown areas", True) 20 | gen.add("laser_min_angle_deg", double_t, 1, "simulated laser beam min incidence angle", -90, -180, 180) 21 | gen.add("laser_max_angle_deg", double_t, 1, "simulated laser beam max incidence angle", 90, -180, 180) 22 | gen.add("laser_angle_step_deg", double_t, 1, "simulated laser beam angle step", 1, 0.01, 45) 23 | gen.add("laser_max_dist_m", double_t, 1, "simulated laser beam max distance", 4.0, 0.1, 100) 24 | gen.add("laser_p_false_pos", double_t, 1, "laser false positive probability", 0.05, 0.0, 1.0) 25 | gen.add("laser_p_false_neg", double_t, 1, "laser false positive probability", 0.05, 0.0, 1.0) 26 | 27 | 28 | exit(gen.generate(PACKAGE, "ase_exploration", "Planner")) 29 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/grid/IPlanarGridMap.h: -------------------------------------------------------------------------------- 1 | #ifndef IPLANARGRIDMAP_H 2 | #define IPLANARGRIDMAP_H 3 | 4 | #include "PlanarGridIndex.h" 5 | #include "ExplorationPlanner/kinematics/PlanarPose.h" 6 | 7 | /** 8 | * class IPlanarGridMap 9 | * 10 | */ 11 | 12 | /******************************* Abstract Class **************************** 13 | IPlanarGridMap does not have any pure virtual methods, but its author 14 | defined it as an abstract class, so you should not use it directly. 15 | Inherit from it instead and create only objects from the derived classes 16 | *****************************************************************************/ 17 | 18 | class IPlanarGridMap 19 | { 20 | public: 21 | 22 | // Constructors/Destructors 23 | // 24 | IPlanarGridMap(PlanarPose origin, double meters_per_cell) 25 | : origin_(origin), meters_per_cell_(meters_per_cell) 26 | { 27 | 28 | } 29 | 30 | virtual ~IPlanarGridMap() {} 31 | 32 | /** 33 | * Get the value of origin_ 34 | * @return the value of origin_ 35 | */ 36 | const PlanarPose& getOrigin() const { 37 | return origin_; 38 | } 39 | 40 | double getMetersPerCell() const { 41 | return meters_per_cell_; 42 | } 43 | 44 | /** 45 | * @param wx world x-coordinate to convert to index 46 | * @param wy world y-coordinate to convert to index 47 | * @param idx assigned to index value at (wx,wy) 48 | */ 49 | void worldToMap(double wx, double wy, PlanarGridIndex& idx) const 50 | { 51 | idx.setX( static_cast ((wx - origin_.getX()) / meters_per_cell_) ); 52 | idx.setY( static_cast ((wy - origin_.getY()) / meters_per_cell_) ); 53 | } 54 | 55 | 56 | /** 57 | * @param idx index of cell *center* to convert to world coordinates 58 | * @param wx assigned to x-coordinate at index cell's *center point* 59 | * @param wy assigned to y-coordinate at index cell's *center point* 60 | */ 61 | void mapToWorld(PlanarGridIndex idx, double& wx, double& wy) const 62 | { 63 | wx = origin_.getX() + (idx.getX() + 0.5) * meters_per_cell_; 64 | wy = origin_.getY() + (idx.getY() + 0.5) * meters_per_cell_; 65 | } 66 | 67 | private: 68 | 69 | // Private attributes 70 | // 71 | PlanarPose origin_; // world coordinate for the corner of cell (0,0) 72 | double meters_per_cell_; // resolution 73 | }; 74 | 75 | #endif // IPLANARGRIDMAP_H 76 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/grid/PlanarGridBinaryMap.h: -------------------------------------------------------------------------------- 1 | #ifndef PLANARGRIDBINARYMAP_H 2 | #define PLANARGRIDBINARYMAP_H 3 | #include "IPlanarGridMap.h" 4 | #include "PlanarGridContainer.h" 5 | 6 | /** 7 | * class PlanarGridOccupancyMap 8 | * 9 | */ 10 | class PlanarGridBinaryMap : virtual public IPlanarGridMap, virtual public PlanarGridContainer 11 | { 12 | public: 13 | PlanarGridBinaryMap(PlanarPose origin, double meters_per_cell) 14 | : IPlanarGridMap(origin, meters_per_cell), 15 | PlanarGridContainer() 16 | { 17 | } 18 | 19 | // increase "safety margin" by marking one pixel around each cell with value "true" 20 | void dilate(); 21 | }; 22 | 23 | #endif // PLANARGRIDBINARYMAP_H 24 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/grid/PlanarGridContainer.h: -------------------------------------------------------------------------------- 1 | #ifndef PLANARGRIDCONTAINER_H 2 | #define PLANARGRIDCONTAINER_H 3 | #include "PlanarGridIndex.h" 4 | #include 5 | 6 | /** 7 | * class PlanarGridContainer 8 | * 9 | */ 10 | 11 | // A sparse grid map container for arbitrary data type T. 12 | template 13 | class PlanarGridContainer 14 | { 15 | public: 16 | typedef boost::unordered_map > Container; 17 | 18 | typedef typename Container::const_iterator ContainerConstIter; 19 | typedef typename Container::iterator ContainerIter; 20 | 21 | /** 22 | * @return T const reference to mapped data type 23 | * @param idx index to get data from 24 | */ 25 | const T& getGridValue(const PlanarGridIndex& idx) const 26 | { 27 | // Throws out_of_range if idx does not match any key in M_: 28 | // see: http://www.cplusplus.com/reference/unordered_map/unordered_map/at/ 29 | return M_.at(idx); 30 | } 31 | 32 | /** 33 | * @param idx 34 | * @param v 35 | */ 36 | void setGridValue(const PlanarGridIndex& idx, const T& v) 37 | { 38 | // default constructor needed for T: 39 | // see http://www.cplusplus.com/reference/unordered_map/unordered_map/operator[]/ 40 | M_[idx] = v; 41 | 42 | // Only works if value is *inserted*, does not modify existing: 43 | // see http://www.cplusplus.com/reference/unordered_map/unordered_map/insert/ 44 | //M_.insert( std::make_pair(idx, v) ); 45 | } 46 | 47 | /** 48 | * @param idx 49 | * @param v 50 | */ 51 | bool isInContainer(const PlanarGridIndex& idx) const 52 | { 53 | return ( M_.find(idx) != M_.end() ); 54 | } 55 | 56 | unsigned int size() const 57 | { 58 | return M_.size(); 59 | } 60 | 61 | bool empty() const 62 | { 63 | return M_.empty(); 64 | } 65 | 66 | /** 67 | * @return ContainerType::const_iterator 68 | * @param idx 69 | */ 70 | ContainerConstIter find(const PlanarGridIndex& idx) const 71 | { 72 | return M_.find(idx); 73 | } 74 | 75 | /** 76 | * @return ContainerType::const_iterator 77 | * @param idx 78 | */ 79 | ContainerConstIter begin() const 80 | { 81 | return M_.begin(); 82 | } 83 | 84 | /** 85 | * @return ContainerType::const_iterator 86 | * @param idx 87 | */ 88 | ContainerConstIter end() const 89 | { 90 | return M_.end(); 91 | } 92 | 93 | // Gets minimum and maximum values for the maps indices 94 | bool getMapExtents(int& x_min, int& x_max, int& y_min, int& y_max) const 95 | { 96 | if (M_.empty()) 97 | return false; 98 | 99 | x_min = M_.cbegin()->first.getX(); 100 | x_max = M_.cbegin()->first.getX(); 101 | y_min = M_.cbegin()->first.getY(); 102 | y_max = M_.cbegin()->first.getY(); 103 | for (ContainerConstIter it = M_.cbegin(), iend = M_.cend(); it != iend; ++it) 104 | { 105 | x_min = std::min(x_min, it->first.getX()); 106 | x_max = std::max(x_max, it->first.getX()); 107 | y_min = std::min(y_min, it->first.getY()); 108 | y_max = std::max(y_max, it->first.getY()); 109 | } 110 | return true; 111 | } 112 | 113 | 114 | 115 | private: 116 | // Private attributes 117 | // 118 | Container M_; 119 | }; 120 | 121 | #endif // PLANARGRIDCONTAINER_H 122 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/grid/PlanarGridIndex.h: -------------------------------------------------------------------------------- 1 | #ifndef PLANARGRIDINDEX_H 2 | #define PLANARGRIDINDEX_H 3 | #include 4 | /** 5 | * class PlanarGridIndex 6 | * 7 | */ 8 | 9 | class PlanarGridIndex 10 | { 11 | public: 12 | 13 | // Constructors/Destructors 14 | // 15 | PlanarGridIndex(int x, int y) : x_(x), y_(y) { } 16 | 17 | // Public attributes 18 | // 19 | friend std::size_t hash_value(const PlanarGridIndex& m) 20 | { 21 | std::size_t seed = 0; 22 | boost::hash_combine(seed, m.x_); 23 | boost::hash_combine(seed, m.y_); 24 | return seed; 25 | } 26 | 27 | private: 28 | 29 | // Private attributes 30 | // 31 | 32 | int x_; 33 | int y_; 34 | 35 | public: 36 | // Private attribute accessor methods 37 | // 38 | /** 39 | * Set the value of x_ 40 | * @param x the new value of x_ 41 | */ 42 | void setX(double x) { 43 | x_ = x; 44 | } 45 | 46 | /** 47 | * Set the value of y_ 48 | * @param x the new value of y_ 49 | */ 50 | void setY(double y) { 51 | y_ = y; 52 | } 53 | 54 | /** 55 | * Get the value of x_ 56 | * @return the value of x_ 57 | */ 58 | int getX() const { 59 | return x_; 60 | } 61 | 62 | /** 63 | * Get the value of y_ 64 | * @return the value of y_ 65 | */ 66 | int getY() const { 67 | return y_; 68 | } 69 | 70 | }; 71 | 72 | // Used for hashing 73 | inline bool operator==(const PlanarGridIndex& ma, const PlanarGridIndex& mb) 74 | { 75 | return ( (ma.getX() == mb.getX()) && (ma.getY() == mb.getY()) ); 76 | } 77 | 78 | // Used for hashing 79 | inline bool operator!=(const PlanarGridIndex& ma, const PlanarGridIndex& mb) 80 | { 81 | return !( ma == mb); 82 | } 83 | 84 | 85 | #endif // PLANARGRIDINDEX_H 86 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/grid/PlanarGridOccupancyMap.h: -------------------------------------------------------------------------------- 1 | #ifndef PLANARGRIDOCCUPANCYMAP_H 2 | #define PLANARGRIDOCCUPANCYMAP_H 3 | #include "ExplorationPlanner/grid/IPlanarGridMap.h" 4 | #include "ExplorationPlanner/grid/PlanarGridContainer.h" 5 | #include "ExplorationPlanner/sensor/PlanarObservation.h" 6 | #include "ExplorationPlanner/utils/EntropyLUT.h" 7 | #include "ExplorationPlanner/utils/LogOddsLUT.h" 8 | 9 | #include // int8_t 10 | 11 | /** 12 | * class PlanarGridOccupancyMap 13 | * 14 | */ 15 | class PlanarGridOccupancyMap : virtual public IPlanarGridMap, virtual public PlanarGridContainer 16 | { 17 | public: 18 | // Constructors/Destructors 19 | // 20 | PlanarGridOccupancyMap(PlanarPose origin, double meters_per_cell); 21 | 22 | void update(const PlanarObservation& cells); 23 | 24 | void copyCellsFromOtherMap(const PlanarGridOccupancyMap& other_map, const PlanarObservation& cells); 25 | 26 | /** 27 | * @return double 28 | */ 29 | double getEntropy() const; 30 | 31 | /** 32 | * @return double 33 | * @param cells 34 | * @param prior_map 35 | */ 36 | double getEntropyInCells(const PlanarObservation& cells) const; 37 | 38 | private: 39 | void updateCell(const PlanarGridIndex& idx, const PlanarCellObservation& obs); 40 | EntropyLUT entropy_lut_; 41 | LogOddsLUT logodds_lut_; 42 | static int8_t lo2prob(double l); 43 | }; 44 | 45 | #endif // PLANARGRIDOCCUPANCYMAP_H 46 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/kinematics/IConstraint.h: -------------------------------------------------------------------------------- 1 | #ifndef ICONSTRAINT_H 2 | #define ICONSTRAINT_H 3 | #include 4 | /** 5 | * class IConstraint 6 | * 7 | */ 8 | 9 | class IConstraint 10 | { 11 | public: 12 | virtual ~IConstraint() {} 13 | 14 | virtual std::unique_ptr clone() const = 0; 15 | 16 | /** 17 | * @return bool 18 | * @param value 19 | */ 20 | virtual bool check (double value) const = 0; 21 | 22 | virtual double getMinValue() const = 0; 23 | virtual double getMaxValue() const = 0; 24 | 25 | virtual double forceToNearestLimit(double value) const = 0; 26 | }; 27 | 28 | #endif // ICONSTRAINT_H 29 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/kinematics/IPlanarKinematics.h: -------------------------------------------------------------------------------- 1 | #ifndef IPLANARKINEMATICS_H 2 | #define IPLANARKINEMATICS_H 3 | #include "PlanarPose.h" 4 | #include "PlanarRobotVelCmd.h" 5 | #include "IConstraint.h" 6 | #include 7 | #include 8 | #include 9 | /** 10 | * class IPlanarKinematics 11 | * 12 | */ 13 | 14 | class IPlanarKinematics 15 | { 16 | public: 17 | virtual ~IPlanarKinematics() {} 18 | 19 | virtual std::unique_ptr clone() const = 0; 20 | 21 | /** 22 | * @return bool 23 | * @param cmd 24 | */ 25 | virtual bool checkConstraints(const PlanarRobotVelCmd& cmd, 26 | const PlanarPose& current_pose) const = 0; 27 | 28 | /** 29 | * @return PlanarPose 30 | * @param current_pose 31 | * @param cmd 32 | */ 33 | virtual PlanarPose getNextPose(const PlanarPose& current_pose, const PlanarRobotVelCmd& cmd) const = 0; 34 | virtual void getTrajectory(std::vector& trajectory, 35 | const PlanarPose& start_pose, 36 | const std::vector& commands) const = 0; 37 | 38 | 39 | virtual const IConstraint& getLinearVelC() const = 0; 40 | virtual const IConstraint& getAngularVelC() const = 0; 41 | virtual const IConstraint& getFinalRotC() const = 0; 42 | 43 | protected: 44 | private: 45 | 46 | }; 47 | 48 | #endif // IPLANARKINEMATICS_H 49 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/kinematics/IPlanarRobotVelCmdSampler.h: -------------------------------------------------------------------------------- 1 | #ifndef IPLANARROBOTVELCMDSAMPLER_H 2 | #define IPLANARROBOTVELCMDSAMPLER_H 3 | #include "PlanarRobotVelCmd.h" 4 | #include "IPlanarKinematics.h" 5 | #include "ExplorationPlanner/planner/ExplorationTaskState.h" 6 | #include "ExplorationPlanner/utils/RNG.h" 7 | #include 8 | #include 9 | 10 | /** 11 | * class IPlanarRobotVelCmdSampler 12 | * 13 | */ 14 | class IPlanarRobotVelCmdSampler 15 | { 16 | public: 17 | virtual ~IPlanarRobotVelCmdSampler() {} 18 | 19 | /** 20 | * @return PlanarRobotVelCmd 21 | * @param cmd 22 | */ 23 | 24 | virtual PlanarRobotVelCmd sample(const PlanarRobotVelCmd& old_cmd, 25 | const IPlanarKinematics& kinematics, 26 | RNG &rng) = 0; 27 | virtual std::unique_ptr clone() const = 0; 28 | virtual std::string Print() const = 0; 29 | }; 30 | 31 | #endif // IPLANARROBOTVELCMDSAMPLER_H 32 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/kinematics/PlanarPose.h: -------------------------------------------------------------------------------- 1 | #ifndef PLANARPOSE_H 2 | #define PLANARPOSE_H 3 | #include 4 | /** 5 | * class PlanarPose 6 | * 7 | */ 8 | 9 | class PlanarPose 10 | { 11 | public: 12 | 13 | // Constructors/Destructors 14 | // 15 | 16 | PlanarPose() 17 | : x_(0.0), y_(0.0), theta_(0.0) 18 | { 19 | } 20 | 21 | PlanarPose(double x, double y, double theta) 22 | : x_(x), y_(y), theta_(theta) 23 | { 24 | } 25 | 26 | // copy ctor 27 | PlanarPose(const PlanarPose& p) 28 | : x_(p.x_), y_(p.y_), theta_(p.theta_) 29 | { 30 | 31 | } 32 | 33 | std::string Print() const; 34 | 35 | private: 36 | double x_; 37 | double y_; 38 | double theta_; 39 | 40 | public: 41 | 42 | 43 | // Private attribute accessor methods 44 | // 45 | /** 46 | * Get the value of x_ 47 | * @return the value of x_ 48 | */ 49 | double getX() const { 50 | return x_; 51 | } 52 | 53 | /** 54 | * Get the value of y_ 55 | * @return the value of y_ 56 | */ 57 | double getY() const { 58 | return y_; 59 | } 60 | 61 | /** 62 | * Get the value of theta_ 63 | * @return the value of theta_ 64 | */ 65 | double getTheta() const { 66 | return theta_; 67 | } 68 | }; 69 | 70 | #endif // PLANARPOSE_H 71 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/kinematics/PlanarRobotVelCmd.h: -------------------------------------------------------------------------------- 1 | #ifndef PLANARROBOTVELCMD_H 2 | #define PLANARROBOTVELCMD_H 3 | #include 4 | 5 | /** 6 | * class PlanarRobotVelCmd 7 | * 8 | */ 9 | 10 | class PlanarRobotVelCmd 11 | { 12 | public: 13 | 14 | // Constructors/Destructors 15 | // 16 | PlanarRobotVelCmd() 17 | : linear_vel_mps_(0.0), angular_vel_rps_(0.0), 18 | final_rotation_rads(0.0), duration_secs_(0.0) 19 | { 20 | } 21 | 22 | PlanarRobotVelCmd(double vel_lin, double vel_ang, double final_rot, double duration) 23 | : linear_vel_mps_(vel_lin), angular_vel_rps_(vel_ang), 24 | final_rotation_rads(final_rot), duration_secs_(duration) 25 | { 26 | } 27 | 28 | 29 | std::string Print() const; 30 | 31 | // Private attribute accessor methods 32 | // 33 | /** 34 | * Get the value of linear_vel_mps_ 35 | * @return the value of linear_vel_mps_ 36 | */ 37 | double getLinearVelocity () const { 38 | return linear_vel_mps_; 39 | } 40 | 41 | /** 42 | * Get the value of angular_vel_rps_ 43 | * @return the value of angular_vel_rps_ 44 | */ 45 | double getAngularVelocity () const { 46 | return angular_vel_rps_; 47 | } 48 | 49 | /** 50 | * Get the value of final_rotation_rads 51 | * @return the value of final_rotation_rads 52 | */ 53 | double getFinalRotation() const { 54 | return final_rotation_rads; 55 | } 56 | 57 | 58 | /** 59 | * Get the value of duration_secs_ 60 | * @return the value of duration_secs_ 61 | */ 62 | double getDuration() const { 63 | return duration_secs_; 64 | } 65 | 66 | private: 67 | // Private attributes 68 | // 69 | double linear_vel_mps_; 70 | double angular_vel_rps_; 71 | double final_rotation_rads; 72 | double duration_secs_; 73 | }; 74 | 75 | #endif // PLANARROBOTVELCMD_H 76 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/kinematics/PlanarRobotVelCmdSamplerIndependentGaussian.h: -------------------------------------------------------------------------------- 1 | #ifndef PLANARROBOTVELCMDSAMPLERINDEPENDENTGAUSSIAN_H 2 | #define PLANARROBOTVELCMDSAMPLERINDEPENDENTGAUSSIAN_H 3 | #include "VelocityPlanarKinematics.h" 4 | #include "IPlanarRobotVelCmdSampler.h" 5 | #include "ExplorationPlanner/utils/RNG.h" 6 | #include 7 | 8 | /** 9 | * class PlanarRobotVelCmdSamplerIndependentGaussian 10 | * 11 | */ 12 | 13 | class PlanarRobotVelCmdSamplerIndependentGaussian : virtual public IPlanarRobotVelCmdSampler 14 | { 15 | public: 16 | // Constructors/Destructors 17 | // 18 | PlanarRobotVelCmdSamplerIndependentGaussian( 19 | double linear_velocity_stdev, 20 | double angular_velocity_stdev, 21 | double final_rotation_stdev, 22 | double default_duration_secs = 1.0); 23 | 24 | virtual std::unique_ptr clone() const 25 | { 26 | return std::unique_ptr( new PlanarRobotVelCmdSamplerIndependentGaussian(lv_std_, 27 | av_std_, 28 | fr_std_, 29 | default_ctrl_duration_secs_) ); 30 | } 31 | 32 | virtual PlanarRobotVelCmd sample(const PlanarRobotVelCmd& old_cmd, 33 | const IPlanarKinematics& kinematics, 34 | RNG &rng); 35 | 36 | std::string Print() const; 37 | 38 | private: 39 | typedef boost::random::normal_distribution NormalDist; 40 | double default_ctrl_duration_secs_; 41 | double lv_std_; 42 | double av_std_; 43 | double fr_std_; 44 | }; 45 | 46 | #endif // PLANARROBOTVELCMDSAMPLERINDEPENDENTGAUSSIAN_H 47 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/kinematics/PlanarRobotVelCmdSamplerUniform.h: -------------------------------------------------------------------------------- 1 | #ifndef PLANARROBOTVELCMDSAMPLERUNIFORM_H 2 | #define PLANARROBOTVELCMDSAMPLERUNIFORM_H 3 | #include "IPlanarRobotVelCmdSampler.h" 4 | #include "VelocityPlanarKinematics.h" 5 | #include "ExplorationPlanner/utils/RNG.h" 6 | #include 7 | /** 8 | * class PlanarRobotVelCmdSamplerUniform 9 | * 10 | */ 11 | 12 | class PlanarRobotVelCmdSamplerUniform : virtual public IPlanarRobotVelCmdSampler 13 | { 14 | public: 15 | 16 | // Constructors/Destructors 17 | // 18 | PlanarRobotVelCmdSamplerUniform(double default_duration_secs = 1.0) 19 | : default_ctrl_duration_secs_(default_duration_secs) 20 | { 21 | } 22 | 23 | virtual std::unique_ptr clone() const 24 | { 25 | return std::unique_ptr( new PlanarRobotVelCmdSamplerUniform(default_ctrl_duration_secs_)); 26 | } 27 | 28 | virtual PlanarRobotVelCmd sample(const PlanarRobotVelCmd& old_cmd, 29 | const IPlanarKinematics& kinematics, 30 | RNG &rng); 31 | std::string Print() const; 32 | 33 | private: 34 | typedef boost::random::uniform_real_distribution UniDist; 35 | double default_ctrl_duration_secs_; 36 | }; 37 | 38 | #endif // PLANARROBOTVELCMDSAMPLERUNIFORM_H 39 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/kinematics/RangeConstraint.h: -------------------------------------------------------------------------------- 1 | #ifndef RANGECONSTRAINT_H 2 | #define RANGECONSTRAINT_H 3 | #include "IConstraint.h" 4 | /** 5 | * class RangeConstraint 6 | * 7 | */ 8 | 9 | class RangeConstraint : virtual public IConstraint 10 | { 11 | public: 12 | 13 | // Constructors/Destructors 14 | // 15 | RangeConstraint(double rmin, double rmax) 16 | : range_min_(rmin), range_max_(rmax) 17 | { 18 | 19 | } 20 | 21 | virtual std::unique_ptr clone() const; 22 | 23 | /** 24 | * @return bool 25 | * @param value 26 | */ 27 | bool check(double value) const; 28 | 29 | double getMinValue() const { 30 | return range_min_; 31 | } 32 | double getMaxValue() const { 33 | return range_max_; 34 | } 35 | double forceToNearestLimit(double value) const 36 | { 37 | if (value < range_min_) 38 | return range_min_; 39 | else if ( value > range_max_) 40 | return range_max_; 41 | else 42 | return value; 43 | } 44 | 45 | private: 46 | 47 | // Private attributes 48 | // 49 | double range_min_; 50 | double range_max_; 51 | }; 52 | 53 | #endif // RANGECONSTRAINT_H 54 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/kinematics/VelocityPlanarKinematics.h: -------------------------------------------------------------------------------- 1 | #ifndef VELOCITYPLANARKINEMATICS_H 2 | #define VELOCITYPLANARKINEMATICS_H 3 | #include "IPlanarKinematics.h" 4 | #include "IConstraint.h" 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | /** 11 | * class VelocityPlanarKinematics 12 | * 13 | */ 14 | 15 | class VelocityPlanarKinematics : virtual public IPlanarKinematics 16 | { 17 | public: 18 | 19 | VelocityPlanarKinematics( const IConstraint& vc, 20 | const IConstraint& ac, 21 | const IConstraint& fc); 22 | 23 | virtual std::unique_ptr clone() const; 24 | 25 | /** 26 | * @return bool 27 | * @param cmd 28 | */ 29 | virtual bool checkConstraints(const PlanarRobotVelCmd& cmd, 30 | const PlanarPose& current_pose) const; 31 | 32 | /** 33 | * @return PlanarPose 34 | * @param current_pose 35 | * @param cmd 36 | */ 37 | virtual PlanarPose getNextPose(const PlanarPose& current_pose, const PlanarRobotVelCmd& cmd) const; 38 | 39 | virtual void getTrajectory(std::vector& trajectory, const PlanarPose& start_pose, const std::vector& commands) const; 40 | 41 | 42 | const IConstraint& getLinearVelC() const 43 | { 44 | return *linear_vel_c_; 45 | } 46 | 47 | const IConstraint& getAngularVelC() const 48 | { 49 | return *angular_vel_c_; 50 | } 51 | 52 | const IConstraint& getFinalRotC() const 53 | { 54 | return *final_rot_c_; 55 | } 56 | 57 | private: 58 | std::unique_ptr linear_vel_c_; 59 | std::unique_ptr angular_vel_c_; 60 | std::unique_ptr final_rot_c_; 61 | }; 62 | 63 | #endif // VELOCITYPLANARKINEMATICS_H 64 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/planner/ExplorationTaskState.h: -------------------------------------------------------------------------------- 1 | #ifndef EXPLORATIONTASKSTATE_H 2 | #define EXPLORATIONTASKSTATE_H 3 | #include "ExplorationPlanner/kinematics/PlanarPose.h" 4 | #include "ExplorationPlanner/grid/PlanarGridOccupancyMap.h" 5 | #include "ExplorationPlanner/sensor/PlanarObservation.h" 6 | /** 7 | * class ExplorationTaskState 8 | * 9 | */ 10 | 11 | class ExplorationTaskState 12 | { 13 | public: 14 | 15 | // Constructors/Destructors 16 | // 17 | ExplorationTaskState(PlanarPose robot_pose, PlanarGridOccupancyMap map); 18 | 19 | void updateMap(const PlanarObservation& obs) 20 | { 21 | map_.update(obs); 22 | } 23 | 24 | private: 25 | 26 | // Static Private attributes 27 | // 28 | 29 | // Private attributes 30 | // 31 | 32 | PlanarPose robot_pose_; 33 | PlanarGridOccupancyMap map_; 34 | public: 35 | 36 | 37 | // Private attribute accessor methods 38 | // 39 | /** 40 | * Set the value of robot_pose_ 41 | * @param pose the new value of robot_pose_ 42 | */ 43 | void setPose(const PlanarPose& pose) { 44 | robot_pose_ = pose; 45 | } 46 | 47 | 48 | /** 49 | * Get the value of robot_pose_ 50 | * @return the value of robot_pose_ 51 | */ 52 | PlanarPose getRobotPose() const { 53 | return robot_pose_; 54 | } 55 | 56 | /** 57 | * Get reference to map_ 58 | * @return reference to map_ 59 | */ 60 | const PlanarGridOccupancyMap& getMap() const { 61 | return map_; 62 | } 63 | 64 | }; 65 | 66 | #endif // EXPLORATIONTASKSTATE_H 67 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/planner/ICoolingSchedule.h: -------------------------------------------------------------------------------- 1 | #ifndef ICOOLINGSCHEDULE_H 2 | #define ICOOLINGSCHEDULE_H 3 | #include 4 | #include 5 | 6 | class ICoolingSchedule 7 | { 8 | public: 9 | virtual unsigned int getNumberOfReplicates(unsigned int iteration) = 0; 10 | virtual std::unique_ptr clone() const = 0; 11 | virtual std::string Print() const = 0; 12 | }; 13 | 14 | 15 | 16 | #endif // ICOOLINGSCHEDULE_H 17 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/planner/IPlanningAlgorithm.h: -------------------------------------------------------------------------------- 1 | #ifndef IPLANNINGALGORITHM_H 2 | #define IPLANNINGALGORITHM_H 3 | #include "ExplorationTaskState.h" 4 | #include "ExplorationPlanner/kinematics/PlanarPose.h" 5 | #include 6 | 7 | /** 8 | * class PlanningAlgorithm 9 | * 10 | */ 11 | class IPlanningAlgorithm 12 | { 13 | public: 14 | virtual ~IPlanningAlgorithm() {} 15 | 16 | /** 17 | * @return PlanarPose 18 | * @param initial_state 19 | */ 20 | virtual bool plan(const ExplorationTaskState& state) = 0; 21 | virtual void getPlan(std::vector &plan) const = 0; 22 | }; 23 | 24 | #endif // IPLANNINGALGORITHM_H 25 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/planner/LinearCoolingSchedule.h: -------------------------------------------------------------------------------- 1 | #ifndef LINEARCOOLINGSCHEDULE_H 2 | #define LINEARCOOLINGSCHEDULE_H 3 | #include "ICoolingSchedule.h" 4 | #include 5 | 6 | // The linear cooling schedule returns the number of 7 | // replications as C = a*iteration + b 8 | 9 | class LinearCoolingSchedule : virtual public ICoolingSchedule 10 | { 11 | public: 12 | LinearCoolingSchedule(unsigned int a, unsigned int b) 13 | : a_(a), b_(b) 14 | { 15 | } 16 | 17 | unsigned int getNumberOfReplicates(unsigned int iteration) 18 | { 19 | return a_*iteration + b_; 20 | } 21 | 22 | virtual std::unique_ptr clone() const 23 | { 24 | return std::unique_ptr(new LinearCoolingSchedule(a_, b_) ); 25 | } 26 | 27 | std::string Print() const 28 | { 29 | std::stringstream ss; 30 | ss << "Linear schedule: C = a*x + b\na = " << a_ << ", b = " << b_; 31 | return ss.str(); 32 | } 33 | 34 | private: 35 | unsigned int a_; 36 | unsigned int b_; 37 | 38 | }; 39 | 40 | #endif // LINEARCOOLINGSCHEDULE_H 41 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/planner/Particle.h: -------------------------------------------------------------------------------- 1 | #ifndef PARTICLE_H 2 | #define PARTICLE_H 3 | #include "ExplorationPlanner/kinematics/PlanarRobotVelCmd.h" 4 | #include "ExplorationPlanner/kinematics/IPlanarRobotVelCmdSampler.h" 5 | #include 6 | #include 7 | #include 8 | 9 | /** 10 | * class Particle 11 | * 12 | */ 13 | 14 | class Particle 15 | { 16 | public: 17 | 18 | // Constructors/Destructors 19 | // 20 | Particle() 21 | : weight_(0.0), 22 | trajectory_(), 23 | cmds_() 24 | { 25 | 26 | } 27 | 28 | Particle(double w, 29 | const std::vector& trajectory, 30 | const std::vector& cmds) 31 | : weight_(w), 32 | trajectory_(trajectory), 33 | cmds_(cmds) 34 | { 35 | 36 | } 37 | 38 | std::vector& getTrajectory() { 39 | return trajectory_; 40 | } 41 | 42 | std::vector& getCommands() { 43 | return cmds_; 44 | } 45 | 46 | const std::vector& getTrajectory() const { 47 | return trajectory_; 48 | } 49 | 50 | const std::vector& getCommands() const { 51 | return cmds_; 52 | } 53 | 54 | /** 55 | * Set the value of weight_ 56 | * @param w the new value of weight_ 57 | */ 58 | void setWeight(double w) { 59 | weight_ = w; 60 | } 61 | 62 | /** 63 | * Get the value of weight_ 64 | * @return the value of weight_ 65 | */ 66 | double getWeight() const { 67 | return weight_; 68 | } 69 | 70 | 71 | std::string Print() const; 72 | 73 | private: 74 | // Private attributes 75 | // 76 | double weight_; 77 | std::vector trajectory_; 78 | std::vector cmds_; 79 | }; 80 | 81 | #endif // PARTICLE_H 82 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/planner/ParticleSet.h: -------------------------------------------------------------------------------- 1 | #ifndef PARTICLESET_H 2 | #define PARTICLESET_H 3 | 4 | #include "Particle.h" 5 | #include "ExplorationPlanner/utils/RNG.h" 6 | #include "ExplorationPlanner/trajectory/TrajectoryValueHandler.h" 7 | #include 8 | 9 | /** 10 | * class ParticleSet 11 | * 12 | */ 13 | 14 | class ParticleSet 15 | { 16 | public: 17 | // Constructors/Destructors 18 | // 19 | ParticleSet() 20 | { 21 | 22 | } 23 | 24 | ParticleSet(unsigned int n, const Particle& p) 25 | : particles_(n, p) 26 | { 27 | normalizeWeights(); 28 | } 29 | 30 | Particle& operator[] ( unsigned int i ) 31 | { 32 | if ( i >= particles_.size() ) 33 | throw std::out_of_range("Requested particle index out of range"); 34 | return particles_[i]; 35 | } 36 | 37 | /** 38 | */ 39 | void resample(RNG& rng); 40 | 41 | /** 42 | */ 43 | void normalizeWeights(); 44 | 45 | /** 46 | * @return double 47 | */ 48 | double getEffectiveNumberOfParticles() const; 49 | 50 | /** 51 | * @return unsigned int 52 | */ 53 | unsigned int getNumberOfParticles() const { 54 | return particles_.size(); } 55 | 56 | std::string Print() const; 57 | 58 | void weightParticles(const std::vector& h); 59 | 60 | Particle getMaxWeightParticle() const; 61 | 62 | private: 63 | /** 64 | * @return double 65 | */ 66 | double getSumOfWeights() const; 67 | 68 | // Private attributes 69 | // 70 | std::vector particles_; 71 | }; 72 | 73 | #endif // PARTICLESET_H 74 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/planner/SDMPlanner.h: -------------------------------------------------------------------------------- 1 | #ifndef SDMPLANNER_H 2 | #define SDMPLANNER_H 3 | #include "IPlanningAlgorithm.h" 4 | 5 | /** 6 | * class SDMPlanner 7 | * 8 | */ 9 | 10 | /******************************* Abstract Class **************************** 11 | SDMPlanner does not have any pure virtual methods, but its author 12 | defined it as an abstract class, so you should not use it directly. 13 | Inherit from it instead and create only objects from the derived classes 14 | *****************************************************************************/ 15 | 16 | class SDMPlanner : virtual public IPlanningAlgorithm 17 | { 18 | public: 19 | 20 | // Constructors/Destructors 21 | // 22 | SDMPlanner(unsigned int horizon, double discount) 23 | : IPlanningAlgorithm(), 24 | horizon_(horizon), 25 | discount_(discount) 26 | { 27 | 28 | } 29 | 30 | private: 31 | 32 | // Private attributes 33 | // 34 | 35 | unsigned int horizon_; 36 | double discount_; 37 | 38 | public: 39 | 40 | 41 | // Private attribute accessor methods 42 | // 43 | 44 | 45 | /** 46 | * Get the value of horizon_ 47 | * @return the value of horizon_ 48 | */ 49 | unsigned int getHorizon() const { 50 | return horizon_; 51 | } 52 | 53 | /** 54 | * Get the value of discount_ 55 | * @return the value of discount_ 56 | */ 57 | double getDiscount() const { 58 | return discount_; 59 | } 60 | 61 | }; 62 | 63 | #endif // SDMPLANNER_H 64 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/planner/SMCPlanner.h: -------------------------------------------------------------------------------- 1 | #ifndef SMCPLANNER_H 2 | #define SMCPLANNER_H 3 | #include "SMCPlannerParameters.h" 4 | #include "ParticleSet.h" 5 | #include "SDMPlanner.h" 6 | #include "ExplorationPlanner/trajectory/TrajectoryEvaluator.h" 7 | #include "ExplorationPlanner/trajectory/TrajectoryGenerator.h" 8 | #include "ExplorationPlanner/utils/RNG.h" 9 | #include 10 | #include 11 | 12 | /** 13 | * class SMCPlanner 14 | * 15 | */ 16 | 17 | class SMCPlanner : virtual public SDMPlanner 18 | { 19 | public: 20 | SMCPlanner(unsigned int horizon, 21 | double discount, 22 | const boost::shared_ptr& generator, 23 | const boost::shared_ptr& evaluator, 24 | const SMCPlannerParameters& params) 25 | : IPlanningAlgorithm(), 26 | SDMPlanner(horizon, discount), 27 | generator_(generator), 28 | evaluator_(evaluator), 29 | parameters_(params.clone()), 30 | particleset_(), 31 | rng_(), 32 | iteration_(0) 33 | { 34 | 35 | } 36 | 37 | bool iterate(const ExplorationTaskState& state); 38 | 39 | 40 | 41 | /** 42 | * @return PlanarPose 43 | * @param initial_state 44 | */ 45 | bool plan(const ExplorationTaskState& state); 46 | void getPlan(std::vector &plan) const; 47 | 48 | void setParameters(const SMCPlannerParameters& p) 49 | { 50 | parameters_ = p.clone(); 51 | } 52 | 53 | 54 | // At least use in debugging phase: get all 55 | const ParticleSet& getParticleSet() const 56 | { 57 | return particleset_; 58 | } 59 | 60 | std::vector getLatestRewardHandlers() const { 61 | return latest_rewards_; 62 | } 63 | 64 | private: 65 | 66 | // Private attributes 67 | // 68 | boost::shared_ptr generator_; 69 | boost::shared_ptr evaluator_; 70 | std::unique_ptr parameters_; 71 | 72 | ParticleSet particleset_; 73 | RNG rng_; 74 | 75 | unsigned int iteration_; 76 | 77 | void initializeParticles(); 78 | bool updateParticles(); 79 | void evaluateParticles(const ExplorationTaskState& state); 80 | 81 | 82 | std::vector latest_rewards_; 83 | }; 84 | 85 | #endif // SMCPLANNER_H 86 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/planner/SMCPlannerParameters.h: -------------------------------------------------------------------------------- 1 | #ifndef SMCPLANNERPARAMETERS_H 2 | #define SMCPLANNERPARAMETERS_H 3 | #include "ICoolingSchedule.h" 4 | #include "ExplorationPlanner/kinematics/IPlanarRobotVelCmdSampler.h" 5 | #include 6 | #include 7 | #include 8 | 9 | class SMCPlannerParameters 10 | { 11 | public: 12 | SMCPlannerParameters(const ICoolingSchedule& schedule, 13 | unsigned int num_particles, 14 | double resampling_threshold_fraction); 15 | 16 | 17 | std::unique_ptr clone() const 18 | { 19 | std::unique_ptr s( new SMCPlannerParameters(*cooling_schedule_, 20 | num_particles_, 21 | resampling_threshold_fraction_) ); 22 | for ( SamplerPtrVec::const_iterator it = kernels_.begin(), iend = kernels_.end(); it != iend; ++it) 23 | s->addKernel( **it ); 24 | return s; 25 | } 26 | 27 | 28 | unsigned int getNumberOfParticles() const 29 | { 30 | return num_particles_; 31 | } 32 | 33 | unsigned int getNumberOfIterations() const 34 | { 35 | return kernels_.size(); 36 | } 37 | 38 | unsigned int getNumberOfReplicates(unsigned int iteration) const 39 | { 40 | return cooling_schedule_->getNumberOfReplicates(iteration); 41 | } 42 | 43 | IPlanarRobotVelCmdSampler& getKernel(unsigned int k) const 44 | { 45 | if (k > kernels_.size()) 46 | throw std::out_of_range("getKernel: Kernel requested out of range"); 47 | return *kernels_[k]; 48 | } 49 | 50 | double getResamplingThreshold() const 51 | { 52 | return resampling_threshold_fraction_; 53 | } 54 | 55 | void addKernel(const IPlanarRobotVelCmdSampler& k); 56 | 57 | 58 | std::string Print() const; 59 | 60 | private: 61 | typedef std::vector > SamplerPtrVec; 62 | 63 | SamplerPtrVec kernels_; 64 | std::unique_ptr cooling_schedule_; 65 | unsigned int num_particles_; 66 | double resampling_threshold_fraction_; 67 | }; 68 | 69 | #endif // SMCPLANNERPARAMETERS_H 70 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/sensor/Bresenham2D.h: -------------------------------------------------------------------------------- 1 | #ifndef BRESENHAM2D_H 2 | #define BRESENHAM2D_H 3 | 4 | /** 5 | * class Bresenham2D 6 | * 7 | */ 8 | // Simple Bresenham algorithm for 2D ray tracing 9 | class Bresenham2D 10 | { 11 | public: 12 | Bresenham2D(); 13 | // From (0,0) to x_stop, y_stop 14 | void initialize(int x_stop, int y_stop); 15 | bool getNext(int& x_next, int& y_next); 16 | 17 | private: 18 | bool initialized_; 19 | 20 | // Internal state for the algorithm; updated for new query and when getting next 21 | int x1, y1, x2, y2; // Current location in trace 22 | int delta_x_, delta_y_; 23 | int error_; 24 | signed char ix, iy; 25 | bool dx_geq_dy_; 26 | }; 27 | 28 | #endif // BRESENHAM2D_H 29 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/sensor/LaserScanner2D.h: -------------------------------------------------------------------------------- 1 | #ifndef LASERSCANNER2D_H 2 | #define LASERSCANNER2D_H 3 | 4 | #include "Raytracer2D.h" 5 | #include "PlanarObservation.h" 6 | #include "ExplorationPlanner/grid/PlanarGridOccupancyMap.h" 7 | #include "ExplorationPlanner/grid/PlanarGridBinaryMap.h" 8 | #include "ExplorationPlanner/grid/PlanarGridIndex.h" 9 | #include "ExplorationPlanner/kinematics/PlanarPose.h" 10 | #include "ExplorationPlanner/utils/RNG.h" 11 | 12 | #include 13 | #include 14 | 15 | /** 16 | * class LaserScanner2D 17 | * 18 | */ 19 | 20 | class LaserScanner2D 21 | { 22 | public: 23 | 24 | // Constructors/Destructors 25 | // 26 | LaserScanner2D(double start_angle_rads, double stop_angle_rads, 27 | double angle_step_rads, double max_distance_meters, 28 | double p_false_pos, double p_false_neg); 29 | 30 | /** 31 | * @return PlanarObservation 32 | * @param pose 33 | * @param map_sample_ 34 | * @param map_prior_ 35 | */ 36 | PlanarObservation sampleMeasurement(const PlanarPose& pose, 37 | const PlanarGridOccupancyMap& map_sample, 38 | const PlanarGridOccupancyMap &map_prior, 39 | const PlanarGridBinaryMap& map_ground_truth, 40 | RNG &rng); 41 | 42 | std::string Print() const; 43 | 44 | private: 45 | 46 | // Private attributes 47 | // 48 | double starting_angle_rads_; 49 | double stopping_angle_rads_; 50 | double angle_step_rads_; 51 | double max_distance_meters_; 52 | Raytracer2D raytracer_; 53 | 54 | int8_t prob_false_positive_; 55 | int8_t prob_false_negative_; 56 | // Alternative representation via log odds for inverse sensor model 57 | double ism_free_; 58 | double ism_occupied_; 59 | 60 | // store incidence angles 61 | std::vector av_; 62 | 63 | 64 | // For sampling cell occupancies and observations 65 | typedef boost::random::uniform_int_distribution UniDist; 66 | 67 | // Private methods 68 | // 69 | bool sampleCellMeasurement(const PlanarGridIndex& idx, 70 | const PlanarGridOccupancyMap& map_sample, 71 | const PlanarGridOccupancyMap &map_prior, 72 | const PlanarGridBinaryMap& map_ground_truth, 73 | RNG &rng) const; 74 | 75 | static double prob2lo(int8_t p); 76 | }; 77 | 78 | #endif // LASERSCANNER2D_H 79 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/sensor/PlanarCellObservation.h: -------------------------------------------------------------------------------- 1 | #ifndef PLANARCELLOBSERVATION_H 2 | #define PLANARCELLOBSERVATION_H 3 | 4 | class PlanarCellObservation 5 | { 6 | public: 7 | PlanarCellObservation() 8 | : o_(false), inverse_sensor_model_(0.0) 9 | { 10 | } 11 | 12 | PlanarCellObservation(bool o, double ism) 13 | : o_(o), inverse_sensor_model_(ism) 14 | { 15 | } 16 | 17 | bool getObservation() const { 18 | return o_; 19 | } 20 | 21 | double getISMValue() const { 22 | return inverse_sensor_model_; 23 | } 24 | 25 | private: 26 | bool o_; 27 | double inverse_sensor_model_; 28 | }; 29 | 30 | #endif // PLANARCELLOBSERVATION_H 31 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/sensor/PlanarObservation.h: -------------------------------------------------------------------------------- 1 | #ifndef PLANAROBSERVATION_H 2 | #define PLANAROBSERVATION_H 3 | #include "ExplorationPlanner/grid/PlanarGridContainer.h" 4 | #include "ExplorationPlanner/grid/PlanarGridIndex.h" 5 | #include "ExplorationPlanner/sensor/PlanarCellObservation.h" 6 | #include 7 | /** 8 | * class PlanarObservation 9 | * 10 | */ 11 | 12 | class PlanarObservation : virtual public PlanarGridContainer 13 | { 14 | 15 | }; 16 | 17 | #endif // PLANAROBSERVATION_H 18 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/sensor/Raytracer2D.h: -------------------------------------------------------------------------------- 1 | #ifndef RAYTRACER2D_H 2 | #define RAYTRACER2D_H 3 | #include "ExplorationPlanner/grid/PlanarGridIndex.h" 4 | #include 5 | 6 | class Raytracer2D 7 | { 8 | public: 9 | void initialize(int x_start, int y_start, int x_stop, int y_stop); 10 | bool getNext(int& x_next, int& y_next); 11 | 12 | unsigned int cacheSize() const { 13 | return rt_cache_.size(); 14 | } 15 | 16 | private: 17 | typedef std::pair RayTracePt; 18 | typedef std::vector RayTracePtVec; 19 | typedef boost::unordered_map > RayTracePtMap; 20 | RayTracePtMap rt_cache_; // store all rays projected in memory for later lookup 21 | 22 | RayTracePtVec::const_iterator ipt_; // points to current pt in raytrace 23 | RayTracePtVec::const_iterator ipt_end_; // points to end pt of current raytrace 24 | int xstart_; // remember where we started the trace 25 | int ystart_; 26 | }; 27 | 28 | #endif /* RAYTRACER_H */ 29 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/trajectory/TrajectoryChecker.h: -------------------------------------------------------------------------------- 1 | #ifndef TRAJECTORYCHECKER_H 2 | #define TRAJECTORYCHECKER_H 3 | #include "ExplorationPlanner/grid/PlanarGridBinaryMap.h" 4 | #include "ExplorationPlanner/kinematics/PlanarPose.h" 5 | #include 6 | #include 7 | 8 | class TrajectoryChecker 9 | { 10 | public: 11 | TrajectoryChecker(const boost::shared_ptr& invalid_cells, 12 | bool allow_unknown_cells) 13 | : invalid_cells_(invalid_cells), 14 | allow_unknown_cells_(allow_unknown_cells) 15 | { 16 | } 17 | 18 | bool isPoseValid(const PlanarPose& pose) const; 19 | bool isTrajectoryValid(const std::vector& poses) const; 20 | bool isIndexValid(const PlanarGridIndex& idx) const; 21 | unsigned int getNumOfCellsWithinDistance(double distance_meters) const; 22 | 23 | private: 24 | boost::shared_ptr invalid_cells_; 25 | bool allow_unknown_cells_; 26 | }; 27 | 28 | #endif /* TRAJECTORYCHECKER_H */ 29 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/trajectory/TrajectoryEvaluator.h: -------------------------------------------------------------------------------- 1 | #ifndef TRAJECTORYEVALUATOR_H 2 | #define TRAJECTORYEVALUATOR_H 3 | #include "TrajectoryValue.h" 4 | #include "ExplorationPlanner/kinematics/PlanarPose.h" 5 | #include "ExplorationPlanner/planner/ExplorationTaskState.h" 6 | #include "ExplorationPlanner/sensor/LaserScanner2D.h" 7 | #include "ExplorationPlanner/utils/RNG.h" 8 | #include 9 | #include 10 | 11 | class TrajectoryEvaluator 12 | { 13 | public: 14 | virtual ~TrajectoryEvaluator() {} 15 | TrajectoryEvaluator(const LaserScanner2D& sensor) 16 | : sensors_(std::max(1, omp_get_max_threads()), sensor) 17 | { 18 | 19 | } 20 | 21 | TrajectoryValue evaluateTrajectory(const std::vector& trajectory, 22 | const PlanarGridOccupancyMap &map, 23 | RNG& rng); 24 | 25 | private: 26 | std::vector sensors_; // each thread has own sensor object 27 | }; 28 | 29 | #endif /* TRAJECTORYEVALUATOR_H */ 30 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/trajectory/TrajectoryGenerator.h: -------------------------------------------------------------------------------- 1 | #ifndef TRAJECTORYGENERATOR_H 2 | #define TRAJECTORYGENERATOR_H 3 | #include "TrajectoryChecker.h" 4 | #include "ExplorationPlanner/kinematics/PlanarPose.h" 5 | #include "ExplorationPlanner/kinematics/PlanarRobotVelCmd.h" 6 | #include "ExplorationPlanner/kinematics/IPlanarRobotVelCmdSampler.h" 7 | #include "ExplorationPlanner/kinematics/IPlanarKinematics.h" 8 | #include "ExplorationPlanner/utils/RNG.h" 9 | #include 10 | 11 | class TrajectoryGenerator 12 | { 13 | public: 14 | TrajectoryGenerator(const IPlanarKinematics& kinematics, 15 | const boost::shared_ptr& checker, 16 | double minimum_trajectory_length, 17 | unsigned int max_tries) 18 | : kinematics_(kinematics.clone()), 19 | checker_(checker), 20 | minimum_trajectory_length_(minimum_trajectory_length), 21 | max_tries_(max_tries) 22 | {} 23 | 24 | 25 | //copy ctor 26 | TrajectoryGenerator(const TrajectoryGenerator& g) 27 | : kinematics_(g.kinematics_->clone()), 28 | checker_(g.checker_), 29 | minimum_trajectory_length_(g.minimum_trajectory_length_), 30 | max_tries_(g.max_tries_) 31 | {} 32 | 33 | 34 | // Fill trajectory and commands 35 | bool generate(std::vector& trajectory, 36 | std::vector& commands, 37 | IPlanarRobotVelCmdSampler& sampler, 38 | RNG& rng) const; 39 | 40 | private: 41 | double minimum_trajectory_length_; 42 | unsigned int max_tries_; 43 | 44 | std::unique_ptr kinematics_; 45 | boost::shared_ptr checker_; 46 | 47 | void getSubParts(std::vector &subparts, 48 | const PlanarPose &start_pose, 49 | const PlanarRobotVelCmd& command) const; 50 | 51 | bool updatePoseAndCmd(PlanarPose& newpose, 52 | PlanarRobotVelCmd& newcmd, 53 | IPlanarRobotVelCmdSampler& sampler, 54 | RNG& rng) const; 55 | 56 | }; 57 | 58 | #endif /* TRAJECTORYGENERATOR_H */ 59 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/trajectory/TrajectoryValue.h: -------------------------------------------------------------------------------- 1 | #ifndef TRAJECTORYVALUE_H 2 | #define TRAJECTORYVALUE_H 3 | #include "ExplorationPlanner/kinematics/PlanarPose.h" 4 | #include "ExplorationPlanner/sensor/PlanarObservation.h" 5 | #include 6 | 7 | class TrajectoryValue 8 | { 9 | public: 10 | TrajectoryValue() : 11 | rewards_(), 12 | observations_() 13 | { 14 | 15 | } 16 | 17 | double getSumOfDiscountedRewards(double discount) const 18 | { 19 | double d = 1.0; 20 | double discounted_sum = 0.0; 21 | for (auto r : rewards_) 22 | { 23 | discounted_sum += d * r; 24 | d *= discount; 25 | } 26 | return discounted_sum; 27 | } 28 | 29 | void insert(const double reward) 30 | { 31 | rewards_.push_back(reward); 32 | } 33 | 34 | void insert(const double reward, const PlanarObservation& observation) 35 | { 36 | rewards_.push_back(reward); 37 | observations_.push_back(observation); 38 | } 39 | 40 | 41 | private: 42 | std::vector rewards_; 43 | std::vector observations_; 44 | }; 45 | 46 | #endif /* TRAJECTORYVALUE_H */ 47 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/trajectory/TrajectoryValueHandler.h: -------------------------------------------------------------------------------- 1 | #ifndef TRAJECTORYVALUEHANDLER_H 2 | #define TRAJECTORYVALUEHANDLER_H 3 | #include "TrajectoryValue.h" 4 | #include 5 | #include 6 | 7 | class TrajectoryValueHandler 8 | { 9 | public: 10 | TrajectoryValueHandler(double discount) 11 | : discount_(discount) 12 | { 13 | 14 | } 15 | 16 | void addValue(const TrajectoryValue& v) 17 | { 18 | values_.push_back( v ); 19 | } 20 | 21 | // Returns weight factor for the particle: 22 | // new weight = old weight * weight factor 23 | double getWeightFactor(double subtract, double scaleFactor) const; 24 | double getMinSumOfDiscRew() const; 25 | double getMaxSumOfDiscRew() const; 26 | double getAvgSumOfDiscRew() const; 27 | 28 | private: 29 | double discount_; 30 | std::vector values_; 31 | }; 32 | 33 | #endif /* TRAJECTORYVALUEHANDLER_H */ 34 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/utils/EntropyLUT.h: -------------------------------------------------------------------------------- 1 | #ifndef ENTROPYLUT_H 2 | #define ENTROPYLUT_H 3 | #include 4 | #include 5 | class EntropyLUT 6 | { 7 | public: 8 | double get(int8_t i) const 9 | { 10 | if ((i < 0) || (i > 100)) 11 | std::cout << "EntropyLUT::get(i) -- ERROR: i = " << static_cast(i) << " is not valid, expect errors!\n"; 12 | 13 | return entropy_[i]; 14 | } 15 | 16 | private: 17 | static double entropy_[101]; 18 | }; 19 | 20 | #endif // ENTROPYLUT_H 21 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/utils/LogOddsLUT.h: -------------------------------------------------------------------------------- 1 | #ifndef LOGODDSLUT_H 2 | #define LOGODDSLUT_H 3 | #include 4 | #include 5 | 6 | class LogOddsLUT 7 | { 8 | public: 9 | double get(int8_t i) const 10 | { 11 | if ((i < 0) || (i > 100)) 12 | std::cout << "LogOddsLUT::get(i) -- ERROR: i = " << i << " is not valid, expect errors!\n"; 13 | return log_odds_[i]; 14 | } 15 | 16 | private: 17 | static double log_odds_[101]; 18 | }; 19 | 20 | 21 | #endif // LOGODDSLUT_H 22 | -------------------------------------------------------------------------------- /include/ExplorationPlanner/utils/RNG.h: -------------------------------------------------------------------------------- 1 | #ifndef RNG_H 2 | #define RNG_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | /** 10 | * class RNG 11 | * 12 | */ 13 | 14 | class RNG 15 | { 16 | public: 17 | typedef boost::random::mt19937 Engine; 18 | 19 | RNG() : engines_() 20 | { 21 | const int threads = std::max(1, omp_get_max_threads()); 22 | // Seed first engine with current time, others by using random numbers from previous generator 23 | // not ideal but oh well... 24 | engines_.push_back(Engine(std::time(0))); 25 | for(int seed = 1; seed < threads; ++seed) 26 | { 27 | engines_.push_back(Engine(engines_[seed-1])); 28 | } 29 | } 30 | 31 | void setSeed(unsigned int seed) 32 | { 33 | if (engines_.empty()) 34 | return; 35 | 36 | engines_[0].seed(seed); 37 | for (int i = 1; i < engines_.size(); ++i) 38 | engines_[i].seed( engines_[i-1]() ); 39 | } 40 | 41 | template 42 | typename DistributionType::result_type operator()(DistributionType& d) 43 | { 44 | return d(engines_[omp_get_thread_num()]); 45 | } 46 | 47 | private: 48 | std::vector engines_; 49 | }; 50 | 51 | #endif // RNG_H 52 | -------------------------------------------------------------------------------- /include/ExplorationPlannerROS/ExplorationPlannerROS.h: -------------------------------------------------------------------------------- 1 | #ifndef EXPLORATIONPLANNERROS_H 2 | #define EXPLORATIONPLANNERROS_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | // Map subscribing 13 | #include 14 | #include 15 | 16 | // move_base client 17 | #include 18 | #include 19 | 20 | // explore action server 21 | #include 22 | #include "ase_exploration/ExploreAction.h" 23 | #include "ExplorationPlanner/planner/SMCPlanner.h" 24 | 25 | // Dynamic reconfiguration 26 | #include 27 | #include 28 | 29 | namespace explorationplanner_ros { 30 | 31 | struct PlannerParamsROS 32 | { 33 | bool wait_between_planner_iterations; 34 | 35 | double lin_v_min; 36 | double lin_v_max; 37 | double ang_v_min; 38 | double ang_v_max; 39 | double f_rot_min; 40 | double f_rot_max; 41 | 42 | // trajectory generation specific parameters 43 | double min_traj_length; 44 | double min_reward; 45 | int max_sampling_tries; 46 | 47 | double laser_min_angle_deg; 48 | double laser_max_angle_deg; 49 | double laser_angle_step_deg; 50 | double laser_max_dist_m; 51 | double p_false_pos; 52 | double p_false_neg; 53 | 54 | int horizon; 55 | double discount; 56 | 57 | int schedule_a; 58 | int schedule_b; 59 | int num_particles; 60 | double resample_thresh; 61 | 62 | int num_kernels; 63 | double std_vel; 64 | double std_ang; 65 | double std_fr; 66 | double default_ctrl_duration; 67 | 68 | bool allow_unknown_targets; 69 | 70 | double goal_execution_time; 71 | double frontier_distance_threshold; 72 | 73 | std::string map_frame_id_; // global map and costmap need to be in this frame 74 | std::string base_frame_id_; 75 | }; 76 | 77 | 78 | class ExplorationPlannerROS 79 | { 80 | public: 81 | ExplorationPlannerROS(const std::string name); 82 | 83 | private: 84 | // ROS connectivity 85 | ros::NodeHandle nh_; 86 | ros::NodeHandle private_nh_; 87 | tf::TransformListener tf_listener_; 88 | 89 | // Server for exploration action 90 | void executeCb(const ase_exploration::ExploreGoalConstPtr &goal); 91 | void preemptCb(); 92 | actionlib::SimpleActionServer explore_server_; 93 | ase_exploration::ExploreFeedback feedback_; 94 | 95 | // Client for move_base 96 | actionlib::SimpleActionClient move_client_; 97 | move_base_msgs::MoveBaseGoal move_client_goal_; 98 | void doneMovingCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result); 99 | void feedbackMovingCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback); 100 | boost::mutex move_client_lock_; 101 | 102 | // subscribe to a costmap from move_base: 103 | // costmap is used to check validity of trajectories, 104 | // we maintain it in a sparse format. 105 | ros::Subscriber costmap_sub_; 106 | ros::Subscriber costmap_update_sub_; 107 | boost::mutex costmap_lock_; 108 | void costmapCb(const nav_msgs::OccupancyGrid &map); 109 | void costmapUpdateCb(const map_msgs::OccupancyGridUpdate &map_update); 110 | boost::shared_ptr trajectorycheck_map_; 111 | 112 | // subscribe to a global map, e.g. from SLAM. 113 | // this map is used to plan and simulate observations. 114 | // we maintain it in sparse format. 115 | ros::Subscriber global_map_sub_; 116 | boost::mutex global_map_lock_; 117 | void globalmapCb(const nav_msgs::OccupancyGrid &map); 118 | PlanarGridOccupancyMap global_map_; 119 | 120 | // dynamic reconfigure 121 | void confCb(ase_exploration::PlannerConfig &config, uint32_t level); 122 | dynamic_reconfigure::Server conf_server_; 123 | dynamic_reconfigure::Server::CallbackType confCb_; 124 | 125 | // internal parameters 126 | PlannerParamsROS params_; 127 | void readParameters(); // reads from parameter server 128 | 129 | // state of exploration task 130 | bool moving_; 131 | bool possibly_stuck_; 132 | bool goal_is_frontier_; 133 | ros::Time goal_time_; 134 | 135 | // tools used by the planner 136 | boost::shared_ptr trajgenerator_; 137 | boost::shared_ptr trajevaluator_; 138 | void updateTrajectoryGenerator(); 139 | void updateTrajectoryEvaluator(); 140 | std::unique_ptr buildPlanner() const; 141 | 142 | // publishing the paths considered by the planner at each iteration 143 | void publishPath(const std::vector& p); 144 | ros::Publisher path_pub_; 145 | }; 146 | 147 | } // namespace explorationplanner_ros 148 | 149 | #endif /* EXPLORATIONPLANNERROS_H */ 150 | -------------------------------------------------------------------------------- /launch/move_base.launch.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /launch/simulator_exploration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 38 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /launch/turtlebot_simulator.launch: -------------------------------------------------------------------------------- 1 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /maps/mit.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurimi/ase_exploration/c1b137456518a25b4c193c332c2de05c657bd4ff/maps/mit.png -------------------------------------------------------------------------------- /maps/mit.yaml: -------------------------------------------------------------------------------- 1 | image: mit.png 2 | resolution: 0.05 3 | origin: [0.0, 0.0, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/stage/mit.world: -------------------------------------------------------------------------------- 1 | include "turtlebot.inc" 2 | 3 | define floorplan model 4 | ( 5 | # sombre, sensible, artistic 6 | color "gray30" 7 | 8 | # most maps will need a bounding box 9 | boundary 1 10 | 11 | gui_nose 0 12 | gui_grid 0 13 | gui_outline 0 14 | gripper_return 0 15 | fiducial_return 0 16 | laser_return 1 17 | ) 18 | 19 | resolution 0.05 20 | interval_sim 100 # simulation timestep in milliseconds 21 | 22 | window 23 | ( 24 | size [ 600.0 700.0 ] 25 | center [ 0.0 0.0 ] 26 | rotate [ 0.0 0.0 ] 27 | scale 60 28 | ) 29 | 30 | floorplan 31 | ( 32 | name "mit" 33 | bitmap "../mit.png" 34 | size [ 48.45 67.05 4.0 ] 35 | pose [ 24.225 33.525 0.0 0.0 ] 36 | ) 37 | 38 | # throw in a robot 39 | turtlebot 40 | ( 41 | pose [ 38.5 10.0 0.0 0.0 ] 42 | name "turtlebot" 43 | color "red" 44 | ) 45 | -------------------------------------------------------------------------------- /maps/stage/turtlebot.inc: -------------------------------------------------------------------------------- 1 | define kinect ranger 2 | ( 3 | sensor 4 | ( 5 | range_max 4.0 6 | fov 270.0 7 | samples 540 8 | ) 9 | # generic model properties 10 | color "black" 11 | size [ 0.06 0.15 0.03 ] 12 | ) 13 | 14 | define turtlebot position 15 | ( 16 | pose [ 0.0 0.0 0.0 0.0 ] 17 | 18 | odom_error [0.03 0.03 999999 999999 999999 0.02] 19 | 20 | size [ 0.2552 0.2552 0.40 ] 21 | origin [ 0.0 0.0 0.0 0.0 ] 22 | gui_nose 1 23 | drive "diff" 24 | color "gray" 25 | 26 | kinect(pose [ -0.1 0.0 -0.11 0.0 ]) 27 | ) 28 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ase_exploration 4 | 0.1.0 5 | The ase_exploration package 6 | 7 | Mikko Lauri 8 | BSD 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | Mikko Lauri 21 | 22 | 23 | 24 | 25 | catkin 26 | geometry_msgs 27 | nav_msgs 28 | roscpp 29 | rospy 30 | tf 31 | actionlib 32 | move_base 33 | message_generation 34 | dynamic_reconfigure 35 | frontier_exploration 36 | 37 | geometry_msgs 38 | nav_msgs 39 | roscpp 40 | rospy 41 | tf 42 | actionlib 43 | move_base 44 | message_runtime 45 | dynamic_reconfigure 46 | frontier_exploration 47 | 48 | -------------------------------------------------------------------------------- /param/frontier_exploration.yaml: -------------------------------------------------------------------------------- 1 | #footprint: [[0.1, 0.0], [0.0, 0.1], [0.0, -0.1], [-0.1, 0.0]] 2 | robot_radius: 0.35 3 | 4 | transform_tolerance: 0.5 5 | update_frequency: 5.0 6 | publish_frequency: 5.0 7 | 8 | #must match incoming static map 9 | global_frame: map 10 | robot_base_frame: base_link 11 | resolution: 0.05 12 | 13 | rolling_window: false 14 | track_unknown_space: true 15 | 16 | plugins: 17 | 18 | - {name: static, type: "costmap_2d::StaticLayer"} 19 | - {name: explore_boundary, type: "frontier_exploration::BoundedExploreLayer"} 20 | #Can disable sensor layer if gmapping is fast enough to update scans 21 | - {name: sensor, type: "costmap_2d::ObstacleLayer"} 22 | - {name: inflation, type: "costmap_2d::InflationLayer"} 23 | 24 | static: 25 | #Can pull data from gmapping, map_server or a non-rolling costmap 26 | # might be slam_map_topic if we want to use that 27 | map_topic: map 28 | #move_base/global_costmap/costmap 29 | # map_topic: move_base/global_costmap/costmap 30 | subscribe_to_updates: true 31 | 32 | explore_boundary: 33 | resize_to_boundary: false 34 | frontier_travel_point: centroid 35 | #set to false for gmapping, true if re-exploring a known area 36 | explore_clear_space: false 37 | 38 | sensor: 39 | observation_sources: laser 40 | laser: {data_type: LaserScan, clearing: true, marking: true, topic: scan, inf_is_valid: true, raytrace_range: 4.0, obstacle_range: 4.0} 41 | 42 | inflation: 43 | inflation_radius: 0.35 -------------------------------------------------------------------------------- /param/navigation/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | max_obstacle_height: 0.60 # assume something like an arm is mounted on top of the robot 2 | 3 | # Obstacle Cost Shaping (http://wiki.ros.org/costmap_2d/hydro/inflation) 4 | robot_radius: 0.20 # distance a circular robot should be clear of the obstacle (kobuki: 0.18) 5 | #footprint: [[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]] # if the robot is not circular 6 | 7 | map_type: voxel 8 | 9 | obstacle_layer: 10 | enabled: true 11 | max_obstacle_height: 0.6 12 | origin_z: 0.0 13 | z_resolution: 0.2 14 | z_voxels: 2 15 | unknown_threshold: 15 16 | mark_threshold: 0 17 | combination_method: 1 18 | track_unknown_space: true #true needed for disabling global path planning through unknown space 19 | obstacle_range: 2.5 20 | raytrace_range: 3.0 21 | origin_z: 0.0 22 | z_resolution: 0.2 23 | z_voxels: 2 24 | publish_voxel_map: false 25 | observation_sources: scan bump 26 | scan: 27 | data_type: LaserScan 28 | topic: scan 29 | marking: true 30 | clearing: true 31 | min_obstacle_height: 0.25 32 | max_obstacle_height: 0.35 33 | bump: 34 | data_type: PointCloud2 35 | topic: mobile_base/sensors/bumper_pointcloud 36 | marking: true 37 | clearing: false 38 | min_obstacle_height: 0.0 39 | max_obstacle_height: 0.15 40 | # for debugging only, let's you see the entire voxel grid 41 | 42 | #cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns 43 | inflation_layer: 44 | enabled: true 45 | cost_scaling_factor: 10.0 # exponential rate at which the obstacle cost drops off (default: 10) 46 | inflation_radius: 0.3 # max. distance from an obstacle at which costs are incurred for planning paths. 47 | 48 | static_layer: 49 | enabled: true 50 | 51 | 52 | -------------------------------------------------------------------------------- /param/navigation/dwa_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | 3 | # Robot Configuration Parameters - Kobuki 4 | max_vel_x: 0.5 # 0.55 5 | min_vel_x: 0.0 6 | 7 | max_vel_y: 0.0 # diff drive robot 8 | min_vel_y: 0.0 # diff drive robot 9 | 10 | max_trans_vel: 0.5 # choose slightly less than the base's capability 11 | min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity 12 | trans_stopped_vel: 0.1 13 | 14 | # Warning! 15 | # do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities 16 | # are non-negligible and small in place rotational velocities will be created. 17 | 18 | max_rot_vel: 5.0 # choose slightly less than the base's capability 19 | min_rot_vel: 0.4 # this is the min angular velocity when there is negligible translational velocity 20 | rot_stopped_vel: 0.4 21 | 22 | acc_lim_x: 1.0 # maximum is theoretically 2.0, but we 23 | acc_lim_theta: 2.0 24 | acc_lim_y: 0.0 # diff drive robot 25 | 26 | # Goal Tolerance Parameters 27 | yaw_goal_tolerance: 0.3 # 0.05 28 | xy_goal_tolerance: 0.3 # 0.10 29 | # latch_xy_goal_tolerance: false 30 | 31 | # Forward Simulation Parameters 32 | sim_time: 1.0 # 1.7 33 | vx_samples: 6 # 3 34 | vy_samples: 1 # diff drive robot, there is only one sample 35 | vtheta_samples: 20 # 20 36 | 37 | # Trajectory Scoring Parameters 38 | path_distance_bias: 64.0 # 32.0 - weighting for how much it should stick to the global path plan 39 | goal_distance_bias: 4.0 # 24.0 - wighting for how much it should attempt to reach its goal 40 | occdist_scale: 0.5 # 0.01 - weighting for how much the controller should avoid obstacles 41 | forward_point_distance: 0.325 # 0.325 - how far along to place an additional scoring point 42 | stop_time_buffer: 0.2 # 0.2 - amount of time a robot must stop in before colliding for a valid traj. 43 | scaling_speed: 0.25 # 0.25 - absolute velocity at which to start scaling the robot's footprint 44 | max_scaling_factor: 0.2 # 0.2 - how much to scale the robot's footprint when at speed. 45 | 46 | # Oscillation Prevention Parameters 47 | oscillation_reset_dist: 0.05 # 0.05 - how far to travel before resetting oscillation flags 48 | 49 | # Debugging 50 | publish_traj_pc : true 51 | publish_cost_grid_pc: true 52 | global_frame_id: odom 53 | 54 | 55 | # Differential-drive robot configuration - necessary? 56 | # holonomic_robot: false 57 | -------------------------------------------------------------------------------- /param/navigation/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_footprint 4 | update_frequency: 1.0 5 | publish_frequency: 0.5 6 | static_map: false 7 | transform_tolerance: 0.5 8 | plugins: 9 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 10 | - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} 11 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 12 | 13 | -------------------------------------------------------------------------------- /param/navigation/global_planner_params.yaml: -------------------------------------------------------------------------------- 1 | 2 | GlobalPlanner: # Also see: http://wiki.ros.org/global_planner 3 | old_navfn_behavior: false # Exactly mirror behavior of navfn, use defaults for other boolean parameters, default false 4 | use_quadratic: true # Use the quadratic approximation of the potential. Otherwise, use a simpler calculation, default true 5 | use_dijkstra: true # Use dijkstra's algorithm. Otherwise, A*, default true 6 | use_grid_path: false # Create a path that follows the grid boundaries. Otherwise, use a gradient descent method, default false 7 | 8 | allow_unknown: true # Allow planner to plan through unknown space, default true 9 | #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work 10 | planner_window_x: 0.0 # default 0.0 11 | planner_window_y: 0.0 # default 0.0 12 | default_tolerance: 0.0 # If goal in obstacle, plan to the closest point in radius default_tolerance, default 0.0 13 | 14 | publish_scale: 100 # Scale by which the published potential gets multiplied, default 100 15 | planner_costmap_publish_frequency: 0.0 # default 0.0 16 | 17 | lethal_cost: 253 # default 253 18 | neutral_cost: 50 # default 50 19 | cost_factor: 3.0 # Factor to multiply each cost from costmap by, default 3.0 20 | publish_potential: true # Publish Potential Costmap (this is not like the navfn pointcloud2 potential), default true -------------------------------------------------------------------------------- /param/navigation/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: /base_footprint 4 | update_frequency: 5.0 5 | publish_frequency: 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 4.0 9 | height: 4.0 10 | resolution: 0.05 11 | transform_tolerance: 0.5 12 | plugins: 13 | - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} 14 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} -------------------------------------------------------------------------------- /param/navigation/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | # Move base node parameters. For full documentation of the parameters in this file, please see 2 | # 3 | # http://www.ros.org/wiki/move_base 4 | # 5 | shutdown_costmaps: false 6 | 7 | controller_frequency: 5.0 8 | controller_patience: 3.0 9 | 10 | 11 | planner_frequency: 1.0 12 | planner_patience: 5.0 13 | 14 | oscillation_timeout: 10.0 15 | oscillation_distance: 0.2 16 | 17 | # local planner - default is trajectory rollout 18 | base_local_planner: "dwa_local_planner/DWAPlannerROS" 19 | 20 | base_global_planner: "navfn/NavfnROS" #alternatives: global_planner/GlobalPlanner, carrot_planner/CarrotPlanner 21 | 22 | 23 | #We plan to integrate recovery behaviors for turtlebot but currently those belong to gopher and still have to be adapted. 24 | ## recovery behaviors; we avoid spinning, but we need a fall-back replanning 25 | #recovery_behavior_enabled: true 26 | 27 | #recovery_behaviors: 28 | #- name: 'super_conservative_reset1' 29 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 30 | #- name: 'conservative_reset1' 31 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 32 | #- name: 'aggressive_reset1' 33 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 34 | #- name: 'clearing_rotation1' 35 | #type: 'rotate_recovery/RotateRecovery' 36 | #- name: 'super_conservative_reset2' 37 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 38 | #- name: 'conservative_reset2' 39 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 40 | #- name: 'aggressive_reset2' 41 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 42 | #- name: 'clearing_rotation2' 43 | #type: 'rotate_recovery/RotateRecovery' 44 | 45 | #super_conservative_reset1: 46 | #reset_distance: 3.0 47 | #conservative_reset1: 48 | #reset_distance: 1.5 49 | #aggressive_reset1: 50 | #reset_distance: 0.0 51 | #super_conservative_reset2: 52 | #reset_distance: 3.0 53 | #conservative_reset2: 54 | #reset_distance: 1.5 55 | #aggressive_reset2: 56 | #reset_distance: 0.0 57 | -------------------------------------------------------------------------------- /param/navigation/navfn_global_planner_params.yaml: -------------------------------------------------------------------------------- 1 | 2 | NavfnROS: 3 | visualize_potential: false #Publish potential for rviz as pointcloud2, not really helpful, default false 4 | allow_unknown: true #Specifies whether or not to allow navfn to create plans that traverse unknown space, default true 5 | #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work 6 | planner_window_x: 0.0 #Specifies the x size of an optional window to restrict the planner to, default 0.0 7 | planner_window_y: 0.0 #Specifies the y size of an optional window to restrict the planner to, default 0.0 8 | 9 | default_tolerance: 0.0 #If the goal is in an obstacle, the planer will plan to the nearest point in the radius of default_tolerance, default 0.0 10 | #The area is always searched, so could be slow for big values 11 | -------------------------------------------------------------------------------- /param/planner.yaml: -------------------------------------------------------------------------------- 1 | # these parameters are set at startup 2 | map_frame_id: map 3 | base_frame_id: base_link 4 | 5 | goal_execution_time: 10.0 6 | min_traj_length: 0.2 7 | min_reward: 30.0 8 | max_sampling_tries: 30 9 | frontier_distance_threshold: 2.0 10 | wait_between_planner_iterations: false 11 | 12 | lin_v_min : 0.3 13 | lin_v_max: 1.0 14 | ang_v_min: -1.0 15 | ang_v_max: 1.0 16 | f_rot_min: -0.02 17 | f_rot_max: 0.02 18 | 19 | # the following can also be changed via dynamic_reconfigure 20 | horizon: 3 21 | discount: 1.0 22 | schedule_a: 4 23 | schedule_b: 3 24 | num_kernels: 5 25 | std_vel: 0.2 26 | std_ang: 0.1 27 | std_fr: 0.02 28 | num_particles: 10 29 | resample_thresh: 0.33 30 | 31 | allow_unknown_targets: true 32 | default_ctrl_duration: 1.0 33 | 34 | laser_min_angle_deg: -90.0 35 | laser_max_angle_deg: 90.0 36 | laser_angle_step_deg: 5.0 37 | laser_max_dist_m: 4.0 38 | p_false_pos: 0.05 39 | p_false_neg: 0.05 40 | -------------------------------------------------------------------------------- /rviz/simulator_exploration.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.52931 8 | Tree Height: 895 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.588679 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: "" 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Alpha: 0.7 32 | Class: rviz/Map 33 | Color Scheme: map 34 | Draw Behind: false 35 | Enabled: true 36 | Name: Map 37 | Topic: /map 38 | Unreliable: false 39 | Value: true 40 | - Alpha: 1 41 | Buffer Length: 10 42 | Class: rviz/Path 43 | Color: 25; 255; 0 44 | Enabled: true 45 | Line Style: Lines 46 | Line Width: 0.03 47 | Name: Path 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Topic: /planner_paths 53 | Unreliable: false 54 | Value: true 55 | - Alpha: 1 56 | Class: rviz/RobotModel 57 | Collision Enabled: false 58 | Enabled: true 59 | Links: 60 | All Links Enabled: true 61 | Expand Joint Details: false 62 | Expand Link Details: false 63 | Expand Tree: false 64 | Link Tree Style: Links in Alphabetic Order 65 | base_footprint: 66 | Alpha: 1 67 | Show Axes: false 68 | Show Trail: false 69 | base_link: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | camera_depth_frame: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | camera_depth_optical_frame: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | camera_link: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | Value: true 87 | camera_rgb_frame: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | camera_rgb_optical_frame: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | caster_back_link: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | Value: true 100 | caster_front_link: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | Value: true 105 | cliff_sensor_front_link: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | cliff_sensor_left_link: 110 | Alpha: 1 111 | Show Axes: false 112 | Show Trail: false 113 | cliff_sensor_right_link: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | gyro_link: 118 | Alpha: 1 119 | Show Axes: false 120 | Show Trail: false 121 | mount_asus_xtion_pro_link: 122 | Alpha: 1 123 | Show Axes: false 124 | Show Trail: false 125 | Value: true 126 | plate_bottom_link: 127 | Alpha: 1 128 | Show Axes: false 129 | Show Trail: false 130 | Value: true 131 | plate_middle_link: 132 | Alpha: 1 133 | Show Axes: false 134 | Show Trail: false 135 | Value: true 136 | plate_top_link: 137 | Alpha: 1 138 | Show Axes: false 139 | Show Trail: false 140 | Value: true 141 | pole_bottom_0_link: 142 | Alpha: 1 143 | Show Axes: false 144 | Show Trail: false 145 | Value: true 146 | pole_bottom_1_link: 147 | Alpha: 1 148 | Show Axes: false 149 | Show Trail: false 150 | Value: true 151 | pole_bottom_2_link: 152 | Alpha: 1 153 | Show Axes: false 154 | Show Trail: false 155 | Value: true 156 | pole_bottom_3_link: 157 | Alpha: 1 158 | Show Axes: false 159 | Show Trail: false 160 | Value: true 161 | pole_bottom_4_link: 162 | Alpha: 1 163 | Show Axes: false 164 | Show Trail: false 165 | Value: true 166 | pole_bottom_5_link: 167 | Alpha: 1 168 | Show Axes: false 169 | Show Trail: false 170 | Value: true 171 | pole_kinect_0_link: 172 | Alpha: 1 173 | Show Axes: false 174 | Show Trail: false 175 | Value: true 176 | pole_kinect_1_link: 177 | Alpha: 1 178 | Show Axes: false 179 | Show Trail: false 180 | Value: true 181 | pole_middle_0_link: 182 | Alpha: 1 183 | Show Axes: false 184 | Show Trail: false 185 | Value: true 186 | pole_middle_1_link: 187 | Alpha: 1 188 | Show Axes: false 189 | Show Trail: false 190 | Value: true 191 | pole_middle_2_link: 192 | Alpha: 1 193 | Show Axes: false 194 | Show Trail: false 195 | Value: true 196 | pole_middle_3_link: 197 | Alpha: 1 198 | Show Axes: false 199 | Show Trail: false 200 | Value: true 201 | pole_top_0_link: 202 | Alpha: 1 203 | Show Axes: false 204 | Show Trail: false 205 | Value: true 206 | pole_top_1_link: 207 | Alpha: 1 208 | Show Axes: false 209 | Show Trail: false 210 | Value: true 211 | pole_top_2_link: 212 | Alpha: 1 213 | Show Axes: false 214 | Show Trail: false 215 | Value: true 216 | pole_top_3_link: 217 | Alpha: 1 218 | Show Axes: false 219 | Show Trail: false 220 | Value: true 221 | wheel_left_link: 222 | Alpha: 1 223 | Show Axes: false 224 | Show Trail: false 225 | Value: true 226 | wheel_right_link: 227 | Alpha: 1 228 | Show Axes: false 229 | Show Trail: false 230 | Value: true 231 | Name: RobotModel 232 | Robot Description: robot_description 233 | TF Prefix: "" 234 | Update Interval: 0 235 | Value: true 236 | Visual Enabled: true 237 | - Alpha: 0.7 238 | Class: rviz/Map 239 | Color Scheme: costmap 240 | Draw Behind: false 241 | Enabled: true 242 | Name: Map 243 | Topic: /move_base/global_costmap/costmap 244 | Unreliable: false 245 | Value: true 246 | - Alpha: 1 247 | Axes Length: 1 248 | Axes Radius: 0.1 249 | Class: rviz/Pose 250 | Color: 255; 25; 0 251 | Enabled: true 252 | Head Length: 0.3 253 | Head Radius: 0.1 254 | Name: Pose 255 | Shaft Length: 1 256 | Shaft Radius: 0.05 257 | Shape: Arrow 258 | Topic: /move_base/current_goal 259 | Unreliable: false 260 | Value: true 261 | - Alpha: 1 262 | Buffer Length: 1 263 | Class: rviz/Path 264 | Color: 255; 0; 4 265 | Enabled: true 266 | Line Style: Lines 267 | Line Width: 0.03 268 | Name: Path 269 | Offset: 270 | X: 0 271 | Y: 0 272 | Z: 0 273 | Topic: /move_base/DWAPlannerROS/global_plan 274 | Unreliable: false 275 | Value: true 276 | Enabled: true 277 | Global Options: 278 | Background Color: 0; 0; 0 279 | Fixed Frame: map 280 | Frame Rate: 30 281 | Name: root 282 | Tools: 283 | - Class: rviz/Interact 284 | Hide Inactive Objects: true 285 | - Class: rviz/MoveCamera 286 | - Class: rviz/Select 287 | - Class: rviz/FocusCamera 288 | - Class: rviz/Measure 289 | - Class: rviz/SetInitialPose 290 | Topic: /initialpose 291 | - Class: rviz/SetGoal 292 | Topic: /move_base_simple/goal 293 | - Class: rviz/PublishPoint 294 | Single click: true 295 | Topic: /clicked_point 296 | Value: true 297 | Views: 298 | Current: 299 | Class: rviz/XYOrbit 300 | Distance: 24.2467 301 | Enable Stereo Rendering: 302 | Stereo Eye Separation: 0.06 303 | Stereo Focal Distance: 1 304 | Swap Stereo Eyes: false 305 | Value: false 306 | Focal Point: 307 | X: 2.25709 308 | Y: -2.26394 309 | Z: 3.1217e-05 310 | Name: Current View 311 | Near Clip Distance: 0.01 312 | Pitch: 0.999801 313 | Target Frame: 314 | Value: XYOrbit (rviz) 315 | Yaw: 4.82041 316 | Saved: ~ 317 | Window Geometry: 318 | Displays: 319 | collapsed: false 320 | Height: 1176 321 | Hide Left Dock: false 322 | Hide Right Dock: true 323 | QMainWindow State: 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 324 | Selection: 325 | collapsed: false 326 | Time: 327 | collapsed: false 328 | Tool Properties: 329 | collapsed: false 330 | Views: 331 | collapsed: true 332 | Width: 1920 333 | X: 3840 334 | Y: 24 335 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/grid/PlanarGridBinaryMap.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/grid/PlanarGridBinaryMap.h" 2 | #include 3 | 4 | void PlanarGridBinaryMap::dilate() 5 | { 6 | boost::unordered_set listtrue; 7 | for ( ContainerConstIter it = begin(), iend = end(); it != iend; ++it) 8 | { 9 | if (it->second) 10 | { 11 | PlanarGridIndex idx = it->first; 12 | for (int w = -1; w <= 1; ++w) 13 | for (int h = -1; h <= 1; ++h) 14 | { 15 | if (w == 0 && h == 0) 16 | continue; 17 | PlanarGridIndex addidx(idx.getX()+w, idx.getY()+h); 18 | if ( isInContainer(addidx) ) // don't add any new items to the map 19 | listtrue.insert(addidx); 20 | } 21 | } 22 | } 23 | 24 | // Now mark all the cells as "true" 25 | for( boost::unordered_set::const_iterator it = listtrue.begin(), iend = listtrue.end(); it != iend; ++it) 26 | setGridValue(*it, true); 27 | } 28 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/grid/PlanarGridOccupancyMap.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/grid/PlanarGridOccupancyMap.h" 2 | 3 | // Constructors/Destructors 4 | // 5 | PlanarGridOccupancyMap::PlanarGridOccupancyMap(PlanarPose origin, double meters_per_cell) 6 | : IPlanarGridMap(origin, meters_per_cell) 7 | { 8 | 9 | } 10 | 11 | // 12 | // Methods 13 | // 14 | 15 | void PlanarGridOccupancyMap::update(const PlanarObservation& cells) 16 | { 17 | for(PlanarObservation::ContainerConstIter it = cells.begin(), iend = cells.end(); it != iend; ++it) 18 | { 19 | updateCell(it->first, it->second); 20 | } 21 | } 22 | 23 | void PlanarGridOccupancyMap::copyCellsFromOtherMap(const PlanarGridOccupancyMap& other_map, const PlanarObservation& cells) 24 | { 25 | for(PlanarObservation::ContainerConstIter it = cells.begin(), iend = cells.end(); it != iend; ++it) 26 | { 27 | ContainerConstIter this_map_it = find( it->first ); 28 | if ( this_map_it == end() ) // Cell is not in map, try to add it 29 | { 30 | // we can only copy from the other map if the cell is present there 31 | ContainerConstIter other_map_it = other_map.find(it->first); 32 | if ( other_map_it != other_map.end() ) 33 | { 34 | setGridValue(it->first, other_map_it->second); // set value for this 35 | } 36 | } 37 | } 38 | } 39 | 40 | void PlanarGridOccupancyMap::updateCell(const PlanarGridIndex& idx, const PlanarCellObservation &obs) 41 | { 42 | ContainerConstIter map_it = find( idx ); 43 | if ( map_it != end() ) // Cell is in map. 44 | { 45 | // log odds representation of (occupancy prob) + (inverse sensor model) 46 | const int8_t new_p = lo2prob( logodds_lut_.get(map_it->second) + obs.getISMValue() ); 47 | setGridValue(idx, new_p); 48 | } 49 | else 50 | { // New cell needs to be inserted 51 | setGridValue(idx, lo2prob(obs.getISMValue()) ); 52 | } 53 | } 54 | 55 | double PlanarGridOccupancyMap::getEntropy() const 56 | { 57 | double e = 0.0; 58 | 59 | for (ContainerConstIter it = begin(), iend = end(); it != iend; ++it) 60 | e += entropy_lut_.get(it->second); 61 | return e; 62 | } 63 | 64 | 65 | double PlanarGridOccupancyMap::getEntropyInCells(const PlanarObservation& cells) const 66 | { 67 | // Compute entropy in specified cells: Use map sample, or if not availabe, use prior map. 68 | double e = 0.0; 69 | for(PlanarObservation::ContainerConstIter it = cells.begin(), iend = cells.end(); it != iend; ++it) 70 | { 71 | ContainerConstIter map_it = find(it->first); 72 | if ( map_it != end() ) 73 | e += entropy_lut_.get(map_it->second); 74 | else 75 | e += 1.0; // Unknown cell, entropy of 1 bit 76 | } 77 | return e; 78 | } 79 | 80 | int8_t PlanarGridOccupancyMap::lo2prob(double l) 81 | { 82 | return static_cast( 100 * (1 - 1 / (1 + exp(l))) ); 83 | } 84 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/kinematics/PlanarPose.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/kinematics/PlanarPose.h" 2 | #include 3 | // Constructors/Destructors 4 | // 5 | 6 | // 7 | // Methods 8 | // 9 | std::string PlanarPose::Print() const 10 | { 11 | std::stringstream ss; 12 | ss << "Planar pose\n"; 13 | ss << " x : " << x_ << "\n"; 14 | ss << " y : " << y_ << "\n"; 15 | ss << " theta: " << theta_; 16 | return ss.str(); 17 | } 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/kinematics/PlanarRobotVelCmd.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/kinematics/PlanarRobotVelCmd.h" 2 | #include 3 | 4 | std::string PlanarRobotVelCmd::Print() const 5 | { 6 | std::stringstream ss; 7 | ss << "Planar robot velocity command\n"; 8 | ss << " Linear velocity : " << linear_vel_mps_ << " m/s\n"; 9 | ss << " Angular velocity: " << angular_vel_rps_ << " rad/s\n"; 10 | ss << " Final rotation : " << final_rotation_rads << " rad/s\n"; 11 | ss << " Duration : " << duration_secs_ << " s\n"; 12 | return ss.str(); 13 | } 14 | 15 | 16 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/kinematics/PlanarRobotVelCmdSamplerIndependentGaussian.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/kinematics/PlanarRobotVelCmdSamplerIndependentGaussian.h" 2 | #include 3 | 4 | 5 | // Constructors/Destructors 6 | // 7 | PlanarRobotVelCmdSamplerIndependentGaussian::PlanarRobotVelCmdSamplerIndependentGaussian(double linear_velocity_stdev, 8 | double angular_velocity_stdev, 9 | double final_rotation_stdev, 10 | double default_duration_secs) 11 | : default_ctrl_duration_secs_(default_duration_secs), 12 | lv_std_(linear_velocity_stdev), 13 | av_std_(angular_velocity_stdev), 14 | fr_std_(final_rotation_stdev) 15 | { 16 | } 17 | 18 | PlanarRobotVelCmd PlanarRobotVelCmdSamplerIndependentGaussian::sample(const PlanarRobotVelCmd& old_cmd, 19 | const IPlanarKinematics& kinematics, 20 | RNG &rng) 21 | { 22 | NormalDist dv( old_cmd.getLinearVelocity(), lv_std_); 23 | NormalDist da( old_cmd.getAngularVelocity(), av_std_); 24 | NormalDist dr( old_cmd.getFinalRotation(), fr_std_); 25 | 26 | const double v = kinematics.getLinearVelC().forceToNearestLimit( rng(dv) ); 27 | const double a = kinematics.getAngularVelC().forceToNearestLimit( rng(da) ); 28 | const double r = kinematics.getFinalRotC().forceToNearestLimit( rng(dr) ); 29 | return PlanarRobotVelCmd(v, a, r, default_ctrl_duration_secs_ ); 30 | } 31 | 32 | std::string PlanarRobotVelCmdSamplerIndependentGaussian::Print() const 33 | { 34 | std::stringstream ss; 35 | ss << "Independent Gaussian kernel\n"; 36 | ss << "Standard deviations:\n"; 37 | ss << " Linear velocity : " << lv_std_ << "\n"; 38 | ss << " Angular velocity: " << av_std_ << "\n"; 39 | ss << " Final rotation : " << fr_std_ << "\n"; 40 | return ss.str(); 41 | } 42 | 43 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/kinematics/PlanarRobotVelCmdSamplerUniform.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/kinematics/PlanarRobotVelCmdSamplerUniform.h" 2 | #include 3 | 4 | PlanarRobotVelCmd PlanarRobotVelCmdSamplerUniform::sample(const PlanarRobotVelCmd& old_cmd, 5 | const IPlanarKinematics& kinematics, 6 | RNG &rng) 7 | { 8 | UniDist dv( kinematics.getLinearVelC().getMinValue(), kinematics.getLinearVelC().getMaxValue() ); 9 | UniDist da( kinematics.getAngularVelC().getMinValue(), kinematics.getAngularVelC().getMaxValue() ); 10 | UniDist dr( kinematics.getFinalRotC().getMinValue(), kinematics.getFinalRotC().getMaxValue() ); 11 | 12 | return PlanarRobotVelCmd( rng(dv), 13 | rng(da), 14 | rng(dr), 15 | default_ctrl_duration_secs_ ); 16 | 17 | } 18 | 19 | std::string PlanarRobotVelCmdSamplerUniform::Print() const 20 | { 21 | std::stringstream ss; 22 | ss << "Uniform kernel on space of allowed controls"; 23 | return ss.str(); 24 | } 25 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/kinematics/RangeConstraint.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/kinematics/RangeConstraint.h" 2 | 3 | bool RangeConstraint::check(double value) const 4 | { 5 | return ( (value >= range_min_ ) && (value <= range_max_) ); 6 | } 7 | 8 | std::unique_ptr RangeConstraint::clone() const 9 | { 10 | return std::unique_ptr( new RangeConstraint(range_min_, range_max_)); 11 | } 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/kinematics/VelocityPlanarKinematics.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/kinematics/VelocityPlanarKinematics.h" 2 | #include "ExplorationPlanner/grid/PlanarGridIndex.h" 3 | #include 4 | 5 | VelocityPlanarKinematics::VelocityPlanarKinematics(const IConstraint& vc, const IConstraint &ac, const IConstraint &fc) 6 | : linear_vel_c_(vc.clone()), 7 | angular_vel_c_(ac.clone()), 8 | final_rot_c_(fc.clone()) 9 | { 10 | } 11 | 12 | std::unique_ptr VelocityPlanarKinematics::clone() const 13 | { 14 | return std::unique_ptr( new VelocityPlanarKinematics( *linear_vel_c_, 15 | *angular_vel_c_, 16 | *final_rot_c_) ); 17 | } 18 | 19 | bool VelocityPlanarKinematics::checkConstraints(const PlanarRobotVelCmd& cmd, 20 | const PlanarPose& current_pose) const 21 | { 22 | return ( linear_vel_c_->check( cmd.getLinearVelocity() ) && 23 | angular_vel_c_->check( cmd.getAngularVelocity()) && 24 | final_rot_c_->check( cmd.getFinalRotation() )); 25 | } 26 | 27 | 28 | PlanarPose VelocityPlanarKinematics::getNextPose(const PlanarPose& current_pose, const PlanarRobotVelCmd& cmd) const 29 | { 30 | const double angvel = cmd.getAngularVelocity(); 31 | // get ratio of linear velocity and angular velocity 32 | double vw = cmd.getLinearVelocity(); 33 | if ( angvel != 0.0 ) 34 | vw /= angvel; 35 | 36 | const double dt = cmd.getDuration(); 37 | const double old_theta = current_pose.getTheta(); 38 | const double next_x = current_pose.getX() - vw * sin(old_theta) + vw * sin(old_theta + angvel*dt); 39 | const double next_y = current_pose.getY() + vw * cos(old_theta) - vw * cos(old_theta + angvel*dt); 40 | double next_theta = current_pose.getTheta() + angvel * dt + cmd.getFinalRotation() * dt; 41 | 42 | // Unwrap the theta angle if necessary 43 | while ( next_theta > boost::math::constants::pi() ) 44 | next_theta -= 2*boost::math::constants::pi(); 45 | while ( next_theta < -boost::math::constants::pi() ) 46 | next_theta += 2*boost::math::constants::pi(); 47 | 48 | return PlanarPose(next_x, next_y, next_theta); 49 | } 50 | 51 | void VelocityPlanarKinematics::getTrajectory(std::vector& trajectory, const PlanarPose& start_pose, const std::vector& commands) const 52 | { 53 | std::size_t sc = commands.size(); 54 | trajectory.resize(sc+1, start_pose); 55 | trajectory[0] = start_pose; 56 | for (std::size_t is = 0; is < sc; ++is) 57 | trajectory[is+1] = getNextPose(trajectory[is], commands[is]); 58 | } 59 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/planner/ExplorationTaskState.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/planner/ExplorationTaskState.h" 2 | 3 | // Constructors/Destructors 4 | // 5 | ExplorationTaskState::ExplorationTaskState(PlanarPose robot_pose, PlanarGridOccupancyMap map) 6 | : robot_pose_(robot_pose), 7 | map_(map) 8 | { 9 | 10 | } 11 | 12 | // 13 | // Methods 14 | // 15 | 16 | 17 | // Accessor methods 18 | // 19 | 20 | 21 | // Other methods 22 | // 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/planner/ICoolingSchedule.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/planner/ICoolingSchedule.h" 2 | #include 3 | 4 | std::ostream &operator<<(std::ostream &os, const ICoolingSchedule& s) 5 | { 6 | os << s.Print(); 7 | return os; 8 | } 9 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/planner/Particle.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/planner/Particle.h" 2 | #include 3 | // Constructors/Destructors 4 | // 5 | 6 | // 7 | // Methods 8 | // 9 | //void Particle::update(const PlanarPose &pose, 10 | // const PlanarGridOccupancyMap& map, 11 | // IPlanarRobotVelCmdSampler &k, 12 | // const IPlanarKinematics& kinematics, 13 | // RNG &rng) 14 | //{ 15 | // PlanarPose trajPose(pose); 16 | // for( std::vector::iterator it = cmds_.begin(), iend = cmds_.end(); it != iend; ++it) 17 | // *it = k.sample(trajPose, map, *it, kinematics, rng); // trajPose will update for each control 18 | //} 19 | 20 | std::string Particle::Print() const 21 | { 22 | std::stringstream ss; 23 | ss << "Weight " << weight_ << "\n"; 24 | const unsigned int sz = cmds_.size(); 25 | ss << "Number of controls: " << sz << "\n"; 26 | for(unsigned int i = 0; i < sz; ++i) 27 | ss << cmds_[i].Print() << "\n"; 28 | const unsigned int st = trajectory_.size(); 29 | ss << "Number of trajectory parts: " << st << "\n"; 30 | for(unsigned int i = 0; i < st; ++i) 31 | ss << trajectory_[i].Print() << "\n"; 32 | return ss.str(); 33 | } 34 | 35 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/planner/ParticleSet.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/planner/ParticleSet.h" 2 | #include 3 | #include 4 | #include 5 | 6 | void ParticleSet::weightParticles(const std::vector& h) 7 | { 8 | assert( h.size() == getNumberOfParticles() ); 9 | // Get min+max rewards 10 | double rmin = std::numeric_limits::max(); 11 | double rmax = -std::numeric_limits::max(); 12 | for ( auto it = h.begin(), iend = h.end(); it != iend; ++it) 13 | { 14 | rmin = std::min(rmin, it->getMinSumOfDiscRew() ); 15 | rmax = std::max(rmax, it->getMaxSumOfDiscRew() ); 16 | } 17 | 18 | double scaleFactor = (rmax - rmin); 19 | if (rmax == rmin) 20 | scaleFactor = 1; 21 | 22 | // Now we can scale the particles 23 | for ( unsigned int i = 0; i < getNumberOfParticles(); ++i ) 24 | { 25 | double w = h[i].getWeightFactor(rmin, scaleFactor); 26 | particles_[i].setWeight( w * particles_[i].getWeight() ); 27 | } 28 | normalizeWeights(); 29 | } 30 | 31 | void ParticleSet::resample(RNG& rng) 32 | { 33 | if (getNumberOfParticles() <= 1) 34 | return; 35 | 36 | const double interval = getSumOfWeights() / getNumberOfParticles(); 37 | boost::random::uniform_01 d; 38 | double target = interval * rng(d); //Initial target weight 39 | 40 | // Compute resampled vector 41 | std::vector resampledParticles(getNumberOfParticles(), particles_.front() ); 42 | double cumw = 0; 43 | unsigned int n = 0; 44 | for (auto it=particles_.begin(), iend=particles_.end(); it != iend; ++it) 45 | { 46 | cumw += it->getWeight(); 47 | while (cumw > target) 48 | { 49 | resampledParticles[n++] = *it; // Postfix increment is crucial. 50 | target += interval; 51 | } 52 | } 53 | 54 | // Assign the new particles and normalize weights 55 | particles_ = resampledParticles; 56 | normalizeWeights(); 57 | } 58 | 59 | Particle ParticleSet::getMaxWeightParticle() const 60 | { 61 | Particle p; 62 | double w = -std::numeric_limits::max(); 63 | for (auto it = particles_.begin(), iend = particles_.end(); it != iend; ++it) 64 | { 65 | double cw = it->getWeight(); 66 | if (w < cw) 67 | { 68 | w = cw; 69 | p = *it; 70 | } 71 | } 72 | return p; 73 | } 74 | 75 | double ParticleSet::getEffectiveNumberOfParticles() const 76 | { 77 | double w_sq = 0.0; 78 | for (auto it = particles_.begin(), iend = particles_.end(); it != iend; ++it) 79 | w_sq += std::pow(it->getWeight(), 2); 80 | return (1.0/w_sq); 81 | } 82 | 83 | void ParticleSet::normalizeWeights() 84 | { 85 | double w = getSumOfWeights(); 86 | for (auto it = particles_.begin(), iend = particles_.end(); it != iend; ++it) 87 | it->setWeight( it->getWeight() / w ); 88 | } 89 | 90 | double ParticleSet::getSumOfWeights() const 91 | { 92 | double w = 0.0; 93 | for (auto it = particles_.begin(), iend = particles_.end(); it != iend; ++it) 94 | w += it->getWeight(); 95 | return w; 96 | } 97 | 98 | std::string ParticleSet::Print() const 99 | { 100 | std::stringstream ss; 101 | const unsigned int sz = particles_.size(); 102 | ss << "Particle set with " << sz << " particles\n"; 103 | for (unsigned int i = 0; i < sz; ++i) 104 | ss << "Particle " << i << ":\n" << particles_[i].Print() << "\n"; 105 | return ss.str(); 106 | } 107 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/planner/SMCPlanner.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/planner/SMCPlanner.h" 2 | #include "ExplorationPlanner/trajectory/TrajectoryValueHandler.h" 3 | #include 4 | 5 | bool SMCPlanner::plan(const ExplorationTaskState& state) 6 | { 7 | for(unsigned int iteration = 0; iteration < parameters_->getNumberOfIterations(); ++iteration) 8 | { 9 | if (!iterate(state)) 10 | return false; 11 | } 12 | return true; 13 | } 14 | 15 | void SMCPlanner::getPlan(std::vector& plan) const 16 | { 17 | Particle pmax = particleset_.getMaxWeightParticle(); 18 | plan = pmax.getTrajectory(); 19 | } 20 | 21 | bool SMCPlanner::iterate(const ExplorationTaskState& state) 22 | { 23 | if ( iteration_ > parameters_->getNumberOfIterations() ) 24 | return false; 25 | 26 | if ( iteration_ == 0 ) 27 | { 28 | // init particleset given the state 29 | const Particle p(1.0, 30 | std::vector(getHorizon()+1, state.getRobotPose()), 31 | std::vector(getHorizon(), PlanarRobotVelCmd()) 32 | ); 33 | particleset_ = ParticleSet(parameters_->getNumberOfParticles(), p); 34 | } 35 | 36 | if (!updateParticles()) 37 | return false; 38 | 39 | evaluateParticles(state); 40 | 41 | ++iteration_; 42 | 43 | return true; 44 | } 45 | 46 | bool SMCPlanner::updateParticles() 47 | { 48 | for (unsigned int i = 0; i < particleset_.getNumberOfParticles(); ++i) 49 | { 50 | if ( !generator_->generate(particleset_[i].getTrajectory(), 51 | particleset_[i].getCommands(), 52 | parameters_->getKernel(iteration_), 53 | rng_)) 54 | std::cout << "Warning, trajectory for particle " << i << " had problems. This should not be critical however...\n"; 55 | } 56 | return true; 57 | } 58 | 59 | void SMCPlanner::evaluateParticles(const ExplorationTaskState& state) 60 | { 61 | const unsigned int num_replications = parameters_->getNumberOfReplicates(iteration_); 62 | 63 | std::vector rewhandlers(particleset_.getNumberOfParticles(), TrajectoryValueHandler(getDiscount())); 64 | #pragma omp parallel for shared(rewhandlers) 65 | for (unsigned int i = 0; i < particleset_.getNumberOfParticles(); ++i) 66 | for(unsigned int replicate = 0; replicate < num_replications; ++replicate) 67 | { 68 | rewhandlers[i].addValue( evaluator_->evaluateTrajectory(particleset_[i].getTrajectory(), state.getMap(), rng_) ); 69 | } 70 | 71 | // assign to latest rewards 72 | latest_rewards_ = rewhandlers; 73 | 74 | // weight the particles 75 | particleset_.weightParticles( latest_rewards_ ); 76 | 77 | // resampling 78 | const double Neff = particleset_.getEffectiveNumberOfParticles(); 79 | const double Neff_thresh = parameters_->getResamplingThreshold() * particleset_.getNumberOfParticles(); 80 | if (Neff < Neff_thresh) 81 | particleset_.resample(rng_); 82 | 83 | } 84 | 85 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/planner/SMCPlannerParameters.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/planner/SMCPlannerParameters.h" 2 | #include 3 | #include 4 | 5 | SMCPlannerParameters::SMCPlannerParameters(const ICoolingSchedule &schedule, 6 | unsigned int num_particles, 7 | double resampling_threshold_fraction) 8 | : cooling_schedule_(schedule.clone()), 9 | num_particles_(num_particles), 10 | resampling_threshold_fraction_(resampling_threshold_fraction) 11 | { 12 | 13 | } 14 | 15 | void SMCPlannerParameters::addKernel(const IPlanarRobotVelCmdSampler &k) 16 | { 17 | kernels_.emplace_back(k.clone()); 18 | } 19 | 20 | 21 | std::string SMCPlannerParameters::Print() const 22 | { 23 | std::stringstream ss; 24 | ss << "::SMC Planner parameters::\n"; 25 | ss << "-Cooling schedule-\n" << cooling_schedule_->Print() << "\n- Kernels -\nNumber of kernels: " << kernels_.size() << "\n"; 26 | for (unsigned int i=0; i < kernels_.size(); ++i) 27 | { 28 | ss << "Kernel " << i << ":\n"; 29 | ss << kernels_[i]->Print() << "\n"; 30 | } 31 | return ss.str(); 32 | } 33 | 34 | // Tells how to print SMCPlannerParameters 35 | std::ostream &operator<<(std::ostream &os, const SMCPlannerParameters& s) 36 | { 37 | os << s.Print(); 38 | return os; 39 | 40 | return os; 41 | } 42 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/sensor/Bresenham2D.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/sensor/Bresenham2D.h" 2 | #include "ExplorationPlanner/grid/PlanarGridIndex.h" 3 | #include 4 | #include 5 | 6 | // Constructors/Destructors 7 | // 8 | Bresenham2D::Bresenham2D() : 9 | initialized_(false), x1(0), y1(0), x2(0), y2(0), delta_x_(0), 10 | delta_y_(0), error_(0), ix(0), iy(0), dx_geq_dy_(false) 11 | { 12 | } 13 | 14 | // 15 | // Methods 16 | // 17 | 18 | void Bresenham2D::initialize(int x_stop, int y_stop) 19 | { 20 | // Update internal state 21 | x1 = 0; 22 | x2 = x_stop; 23 | y1 = 0; 24 | y2 = y_stop; 25 | 26 | delta_x_ = x2 - x1; 27 | // if x1 == x2, then it does not matter what we set here 28 | ix = (delta_x_ > 0) - (delta_x_ < 0); 29 | delta_x_ = static_cast( std::abs(delta_x_) ) << 1; 30 | 31 | delta_y_ = y2 - y1; 32 | // if y1 == y2, then it does not matter what we set here 33 | iy = (delta_y_ > 0) - (delta_y_ < 0); 34 | delta_y_ = static_cast( std::abs(delta_y_) ) << 1; 35 | 36 | dx_geq_dy_ = (delta_x_ >= delta_y_); 37 | 38 | // error may go below zero 39 | if (dx_geq_dy_) 40 | error_ = delta_y_ - (delta_x_ >> 1); 41 | else 42 | error_ = delta_x_ - (delta_y_ >> 1); 43 | 44 | initialized_ = true; 45 | } 46 | 47 | bool Bresenham2D::getNext(int& x_next, int& y_next) 48 | { 49 | if (!initialized_) 50 | return false; 51 | 52 | if (dx_geq_dy_) 53 | { 54 | // As long as x1 != x2, we can go on 55 | if (x1 != x2) 56 | { 57 | if ((error_ >= 0) && (error_ || (ix > 0))) 58 | { 59 | error_ -= delta_x_; 60 | y1 += iy; 61 | } 62 | // else do nothing 63 | 64 | error_ += delta_y_; 65 | x1 += ix; 66 | 67 | // Assign values 68 | x_next = x1; 69 | y_next = y1; 70 | } 71 | else 72 | { 73 | initialized_ = false; 74 | } 75 | } 76 | else 77 | { 78 | // As long as y1 != y2, we can go on 79 | if (y1 != y2) 80 | { 81 | if ((error_ >= 0) && (error_ || (iy > 0))) 82 | { 83 | error_ -= delta_y_; 84 | x1 += ix; 85 | } 86 | // else do nothing 87 | 88 | error_ += delta_x_; 89 | y1 += iy; 90 | 91 | // Assign values 92 | x_next = x1; 93 | y_next = y1; 94 | } 95 | else 96 | { 97 | initialized_ = false; 98 | } 99 | } 100 | return initialized_; 101 | } 102 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/sensor/LaserScanner2D.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/sensor/LaserScanner2D.h" 2 | #include 3 | 4 | // Constructors/Destructors 5 | // 6 | LaserScanner2D::LaserScanner2D(double start_angle_rads, double stop_angle_rads, 7 | double angle_step_rads, double max_distance_meters, 8 | double p_false_pos, double p_false_neg) 9 | : starting_angle_rads_(start_angle_rads), 10 | stopping_angle_rads_(stop_angle_rads), 11 | angle_step_rads_(angle_step_rads), 12 | max_distance_meters_(max_distance_meters), 13 | prob_false_positive_( static_cast(p_false_pos*100)), 14 | prob_false_negative_( static_cast(p_false_neg*100)), 15 | ism_free_( prob2lo(prob_false_positive_) ), 16 | ism_occupied_( prob2lo(100 - prob_false_negative_) ) 17 | { 18 | // Populate list of theoretical beam endpoints from origin 19 | for ( double a = starting_angle_rads_; a < stopping_angle_rads_; a+=angle_step_rads_) 20 | av_.push_back( a ); 21 | } 22 | 23 | // 24 | // Methods 25 | // 26 | PlanarObservation LaserScanner2D::sampleMeasurement(const PlanarPose &pose, 27 | const PlanarGridOccupancyMap& map_sample, 28 | const PlanarGridOccupancyMap& map_prior, 29 | const PlanarGridBinaryMap& map_ground_truth, 30 | RNG& rng) 31 | { 32 | PlanarObservation obs; 33 | PlanarGridIndex start(0,0); 34 | map_sample.worldToMap(pose.getX(), pose.getY(), start); 35 | 36 | for ( auto a : av_ ) 37 | { 38 | // theoretical end point of the beam 39 | const double angle = pose.getTheta() + a; 40 | const double x_end = pose.getX() + max_distance_meters_ * cos(angle); 41 | const double y_end = pose.getY() + max_distance_meters_ * sin(angle); 42 | PlanarGridIndex stop(0,0); 43 | map_sample.worldToMap(x_end, y_end, stop); 44 | 45 | raytracer_.initialize(start.getX(), start.getY(), stop.getX(), stop.getY()); 46 | // Get next raytraced grid cell until the beam terminates or hits an obstacle 47 | int x, y; 48 | while ( raytracer_.getNext(x, y) ) 49 | { 50 | const PlanarGridIndex r(x, y); 51 | bool o = false; // the observation perceived 52 | if (!obs.isInContainer(r)) 53 | { // Observation does not include index; draw a sample 54 | o = sampleCellMeasurement(r, map_sample, map_prior, map_ground_truth, rng); 55 | obs.setGridValue(r, PlanarCellObservation(o, (o ? ism_occupied_ : ism_free_) ) ); 56 | } 57 | else 58 | { // Observation includes the index; get its value 59 | o = obs.find(r)->second.getObservation(); 60 | } 61 | 62 | if (o) 63 | break; // beam hit obstacle 64 | } 65 | 66 | } 67 | return obs; 68 | } 69 | 70 | bool LaserScanner2D::sampleCellMeasurement(const PlanarGridIndex& idx, 71 | const PlanarGridOccupancyMap& map_sample, 72 | const PlanarGridOccupancyMap& map_prior, 73 | const PlanarGridBinaryMap& map_ground_truth, 74 | RNG& rng) const 75 | { 76 | // First try to draw sample from ground truth map, if it is not possible, draw from current map sample, 77 | // then if that can't be done, from current map_prior, and if even that is not possible, 78 | // assume the cell free (unknown). 79 | bool cellOccupancy = false; 80 | if ( !map_ground_truth.empty() && map_ground_truth.isInContainer(idx) ) 81 | { 82 | cellOccupancy = map_ground_truth.getGridValue(idx); 83 | } 84 | else 85 | { 86 | // try to sample an occupancy value for the cell according to current occupancy probability 87 | // priority: current map sample 88 | // backup: current map prior 89 | int8_t cellOccupancyProb = 0; 90 | if ( !map_sample.empty() && map_sample.isInContainer(idx) ) 91 | { 92 | cellOccupancyProb = map_sample.getGridValue(idx); 93 | } 94 | else if ( !map_prior.empty() && map_prior.isInContainer(idx) ) 95 | { 96 | cellOccupancyProb = map_prior.getGridValue(idx); 97 | } 98 | // sampling 99 | boost::random::uniform_int_distribution dc(0,100); 100 | cellOccupancy = (cellOccupancyProb >= rng(dc)); 101 | } 102 | // If neither condition triggers, we assume that the cell is free (do nothing) 103 | 104 | // Sample an observation of the cell given its occupancy value and sensor properties 105 | boost::random::uniform_int_distribution dc(0,100); 106 | const int8_t r = rng(dc); 107 | bool obs(cellOccupancy ? ( r >= prob_false_negative_ ) : ( r <= prob_false_positive_ ) ); 108 | return obs; 109 | } 110 | 111 | 112 | std::string LaserScanner2D::Print() const 113 | { 114 | std::stringstream ss; 115 | ss << "LaserScanner2D: \n" << 116 | " start angle : " << starting_angle_rads_ << " rad\n" << 117 | " stop angle : " << stopping_angle_rads_ << " rad\n" << 118 | " angle step : " << angle_step_rads_ << " rad\n" << 119 | " max distance : " << max_distance_meters_ << " m\n" << 120 | " P(false pos.): " << static_cast(prob_false_positive_) << "\n" << 121 | " P(false neg.): " << static_cast(prob_false_negative_) << "\n" << 122 | " ISM(occupied): " << ism_occupied_ << "\n" << 123 | " ISM(free) : " << ism_free_ << "\n" << 124 | " Cache size : " << raytracer_.cacheSize(); 125 | 126 | return ss.str(); 127 | } 128 | 129 | double LaserScanner2D::prob2lo(int8_t p) 130 | { 131 | double pr = static_cast(p)/100; 132 | return log( pr / (1-pr)); 133 | } 134 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/sensor/Raytracer2D.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/sensor/Raytracer2D.h" 2 | #include "ExplorationPlanner/sensor/Bresenham2D.h" 3 | 4 | // Implementation by projecting all rays to start at origin + cache results 5 | void Raytracer2D::initialize(int x_start, int y_start, int x_stop, int y_stop) 6 | { 7 | // Assign start pt 8 | xstart_ = x_start; 9 | ystart_ = y_start; 10 | 11 | // Transform to start from zero 12 | int xt = (x_stop - x_start); 13 | int yt = (y_stop - y_start); 14 | 15 | PlanarGridIndex idx(xt, yt); 16 | RayTracePtMap::iterator itc = rt_cache_.find(idx); 17 | if ( itc == rt_cache_.end() ) 18 | { 19 | // Precompute to cache, then set the pointer 20 | Bresenham2D br; 21 | br.initialize(xt, yt); 22 | int xn, yn; 23 | RayTracePtVec v; 24 | while (br.getNext(xn, yn)) 25 | v.push_back( RayTracePt(xn, yn) ); 26 | 27 | std::pair in = rt_cache_.insert( std::make_pair(idx, v) ); 28 | itc = in.first; 29 | } 30 | 31 | // Set iterators to point to start and end of appropriate raytrace point vector 32 | ipt_ = itc->second.begin(); 33 | ipt_end_ = itc->second.end(); 34 | } 35 | 36 | bool Raytracer2D::getNext(int& x_next, int& y_next) 37 | { 38 | if (ipt_ == ipt_end_) 39 | return false; 40 | 41 | x_next = xstart_ + ipt_->first; 42 | y_next = ystart_ + ipt_->second; 43 | ++ipt_; 44 | return true; 45 | } 46 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/trajectory/TrajectoryChecker.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/trajectory/TrajectoryChecker.h" 2 | #include 3 | 4 | bool TrajectoryChecker::isPoseValid(const PlanarPose &pose) const 5 | { 6 | PlanarGridIndex idx(0,0); 7 | invalid_cells_->worldToMap(pose.getX(), pose.getY(), idx); 8 | return isIndexValid(idx); 9 | } 10 | 11 | bool TrajectoryChecker::isTrajectoryValid(const std::vector &poses) const 12 | { 13 | for (auto p : poses) 14 | { 15 | if (!isPoseValid(p)) 16 | return false; 17 | } 18 | return true; 19 | } 20 | 21 | bool TrajectoryChecker::isIndexValid(const PlanarGridIndex& idx) const 22 | { 23 | if ( !invalid_cells_->isInContainer(idx) ) // cell is unknown 24 | return allow_unknown_cells_; 25 | 26 | return !(invalid_cells_->getGridValue(idx)); 27 | } 28 | 29 | unsigned int TrajectoryChecker::getNumOfCellsWithinDistance(double distance_meters) const 30 | { 31 | if (distance_meters <= 0.0) 32 | return 1; 33 | 34 | return std::ceil( distance_meters / invalid_cells_->getMetersPerCell() ); 35 | } 36 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/trajectory/TrajectoryEvaluator.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/trajectory/TrajectoryEvaluator.h" 2 | 3 | 4 | TrajectoryValue TrajectoryEvaluator::evaluateTrajectory(const std::vector& trajectory, 5 | const PlanarGridOccupancyMap& map, 6 | RNG& rng) 7 | { 8 | if ( trajectory.size() == 0) 9 | return TrajectoryValue(); 10 | 11 | // start out with an empty map; we will copy elements to it from input map when necessary. 12 | PlanarGridOccupancyMap map_sample(map.getOrigin(), map.getMetersPerCell()); 13 | 14 | // we shall assume an empty ground truth map for this case. 15 | PlanarGridBinaryMap gt(map.getOrigin(), map.getMetersPerCell()); 16 | 17 | TrajectoryValue tv; 18 | for (auto it = trajectory.begin(), iend = trajectory.end(); it != iend; ++it) 19 | { 20 | PlanarObservation observation = sensors_[omp_get_thread_num()].sampleMeasurement(*it, map_sample, map, gt, rng); 21 | // copy all elements from input map to map_sample that are not there yet. 22 | map_sample.copyCellsFromOtherMap(map, observation); 23 | 24 | // compute prior and posterior entropy 25 | double prior_entropy = map_sample.getEntropyInCells(observation); 26 | map_sample.update(observation); 27 | double posterior_entropy = map_sample.getEntropyInCells(observation); 28 | 29 | double mutual_information = (prior_entropy - posterior_entropy); 30 | tv.insert(mutual_information, observation); 31 | } 32 | 33 | return tv; 34 | } 35 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/trajectory/TrajectoryGenerator.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/trajectory/TrajectoryGenerator.h" 2 | 3 | bool TrajectoryGenerator::generate(std::vector &trajectory, 4 | std::vector &commands, 5 | IPlanarRobotVelCmdSampler &sampler, 6 | RNG &rng) const 7 | { 8 | // first item in trajectory is curren pose 9 | std::size_t sz = commands.size(); 10 | if (sz == 0 || sz != (trajectory.size() - 1) ) 11 | { 12 | std::cout << "Unexpected input size for TrajectoryGenerator::generate\n"; 13 | return false; 14 | } 15 | 16 | double total_distance = 0.0; 17 | 18 | for ( std::size_t is = 0; is < sz; ++is) 19 | { 20 | PlanarPose current_pose(trajectory[is]); 21 | PlanarRobotVelCmd current_cmd(commands[is]); 22 | 23 | if ( !updatePoseAndCmd(current_pose, current_cmd, sampler, rng) ) 24 | { 25 | // stay in place 26 | trajectory[is+1] = trajectory[is]; 27 | commands[is] = PlanarRobotVelCmd(0.0, 0.0, 0.0, current_cmd.getDuration()); 28 | } 29 | else 30 | { 31 | trajectory[is+1] = current_pose; 32 | commands[is] = current_cmd; 33 | } 34 | 35 | total_distance += std::abs( commands[is].getLinearVelocity() * commands[is].getDuration() ); 36 | } 37 | 38 | return true; 39 | } 40 | 41 | // newpose and newcmd are only modified if return value is 'true' 42 | bool TrajectoryGenerator::updatePoseAndCmd(PlanarPose& newpose, 43 | PlanarRobotVelCmd& newcmd, 44 | IPlanarRobotVelCmdSampler& sampler, 45 | RNG& rng) const 46 | { 47 | unsigned int retries = max_tries_; 48 | do 49 | { 50 | PlanarRobotVelCmd candidate_cmd = sampler.sample(newcmd, *kinematics_, rng); 51 | std::vector subparts; 52 | getSubParts(subparts, newpose, candidate_cmd); 53 | 54 | if ( checker_->isTrajectoryValid(subparts) ) 55 | { // trajectory is valid, we're done 56 | newpose = subparts.back(); 57 | newcmd = candidate_cmd; 58 | return true; 59 | } 60 | else 61 | { 62 | --retries; 63 | } 64 | } while (retries > 0); 65 | 66 | return false; 67 | } 68 | 69 | 70 | void TrajectoryGenerator::getSubParts(std::vector& subparts, 71 | const PlanarPose& start_pose, 72 | const PlanarRobotVelCmd& command) const 73 | { 74 | double d = std::abs( command.getLinearVelocity() * command.getDuration() ); 75 | unsigned int num_parts = 2 * checker_->getNumOfCellsWithinDistance(d); // safety margin: multiply by two 76 | double dt = command.getDuration() / num_parts; 77 | std::vector split_cmd(num_parts, PlanarRobotVelCmd(command.getLinearVelocity(), 78 | command.getAngularVelocity(), 79 | command.getFinalRotation(), 80 | dt)); 81 | kinematics_->getTrajectory(subparts, start_pose, split_cmd); 82 | } 83 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/trajectory/TrajectoryValue.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurimi/ase_exploration/c1b137456518a25b4c193c332c2de05c657bd4ff/src/ExplorationPlanner/trajectory/TrajectoryValue.cpp -------------------------------------------------------------------------------- /src/ExplorationPlanner/trajectory/TrajectoryValueHandler.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/trajectory/TrajectoryValueHandler.h" 2 | #include 3 | 4 | double TrajectoryValueHandler::getWeightFactor(double subtract, double scaleFactor) const 5 | { 6 | // The scaler should force sums of discounted rewards to the range of 0 ... 1. 7 | // --> Log arguments are between 1 and 2 8 | double w = 0.0; 9 | for (auto v : values_) 10 | w += log( 1 + ((v.getSumOfDiscountedRewards(discount_) - subtract) / scaleFactor) ); 11 | 12 | return exp(w); 13 | } 14 | 15 | double TrajectoryValueHandler::getMinSumOfDiscRew() const 16 | { 17 | double min = std::numeric_limits::max(); 18 | for (auto v : values_) 19 | min = std::min(min, v.getSumOfDiscountedRewards(discount_)); 20 | return min; 21 | } 22 | 23 | double TrajectoryValueHandler::getMaxSumOfDiscRew() const 24 | { 25 | double max = -std::numeric_limits::max(); 26 | for (auto v : values_) 27 | max = std::max(max, v.getSumOfDiscountedRewards(discount_)); 28 | return max; 29 | } 30 | 31 | double TrajectoryValueHandler::getAvgSumOfDiscRew() const 32 | { 33 | double avg = 0.0; 34 | int i = 0; 35 | for (auto v : values_) 36 | avg += (v.getSumOfDiscountedRewards(discount_) - avg) / ( ++i ); 37 | return avg; 38 | } 39 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/utils/EntropyLUT.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/utils/EntropyLUT.h" 2 | 3 | 4 | double EntropyLUT::entropy_[101] = {0, 0.0807931, 0.141441, 0.194392, 0.242292, 0.286397, 0.327445, 0.365924, 5 | 0.402179, 0.43647, 0.468996, 0.499916, 0.529361, 0.557438, 0.584239, 0.60984, 6 | 0.63431, 0.657705, 0.680077, 0.701471, 0.721928, 0.741483, 0.760168, 0.778011, 7 | 0.79504, 0.811278, 0.826746, 0.841465, 0.855451, 0.868721, 0.881291, 0.893173, 8 | 0.904381, 0.914926, 0.924819, 0.934068, 0.942683, 0.950672, 0.958042, 0.9648, 9 | 0.970951, 0.9765, 0.981454, 0.985815, 0.989588, 0.992774, 0.995378, 0.997402, 10 | 0.998846, 0.999711, 1, 0.999711, 0.998846, 0.997402, 0.995378, 0.992774, 0.989588, 11 | 0.985815, 0.981454, 0.9765, 0.970951, 0.9648, 0.958042, 0.950672, 0.942683, 0.934068, 12 | 0.924819, 0.914926, 0.904381, 0.893173, 0.881291, 0.868721, 0.855451, 0.841465, 0.826746, 13 | 0.811278, 0.79504, 0.778011, 0.760168, 0.741483, 0.721928, 0.701471, 0.680077, 0.657705, 14 | 0.63431, 0.60984, 0.584239, 0.557438, 0.529361, 0.499916, 0.468996, 0.43647, 0.402179, 15 | 0.365924, 0.327445, 0.286397, 0.242292, 0.194392, 0.141441, 0.0807931, 0}; 16 | -------------------------------------------------------------------------------- /src/ExplorationPlanner/utils/LogOddsLUT.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlanner/utils/LogOddsLUT.h" 2 | 3 | double LogOddsLUT::log_odds_[101] = {-100, -4.59512, -3.89182, -3.4761, -3.17805, -2.94444, -2.75154, -2.58669, -2.44235, 4 | -2.31363, -2.19722, -2.09074, -1.99243, -1.90096, -1.81529, -1.7346, -1.65823, -1.58563, 5 | -1.51635, -1.45001, -1.38629, -1.32493, -1.26567, -1.20831, -1.15268, -1.09861, -1.04597, 6 | -0.994623, -0.944462, -0.895384, -0.847298, -0.800119, -0.753772, -0.708185, -0.663294, 7 | -0.619039, -0.575364, -0.532217, -0.489548, -0.447312, -0.405465, -0.363965, -0.322773, 8 | -0.281851, -0.241162, -0.200671, -0.160343, -0.120144, -0.0800427, -0.0400053, 0, 0.0400053, 9 | 0.0800427, 0.120144, 0.160343, 0.200671, 0.241162, 0.281851, 0.322773, 0.363965, 0.405465, 10 | 0.447312, 0.489548, 0.532217, 0.575364, 0.619039, 0.663294, 0.708185, 0.753772, 0.800119, 11 | 0.847298, 0.895384, 0.944462, 0.994623, 1.04597, 1.09861, 1.15268, 1.20831, 1.26567, 1.32493, 12 | 1.38629, 1.45001, 1.51635, 1.58563, 1.65823, 1.7346, 1.81529, 1.90096, 1.99243, 2.09074, 2.19722, 13 | 2.31363, 2.44235, 2.58669, 2.75154, 2.94444, 3.17805, 3.4761, 3.89182, 4.59512, 100}; 14 | -------------------------------------------------------------------------------- /src/ExplorationPlannerROS/ExplorationPlannerROS.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "ExplorationPlannerROS/ExplorationPlannerROS.h" 10 | #include "ExplorationPlanner/trajectory/TrajectoryChecker.h" 11 | #include "ExplorationPlanner/trajectory/TrajectoryGenerator.h" 12 | #include "ExplorationPlanner/kinematics/RangeConstraint.h" 13 | #include "ExplorationPlanner/kinematics/VelocityPlanarKinematics.h" 14 | #include "ExplorationPlanner/sensor/LaserScanner2D.h" 15 | #include "ExplorationPlanner/planner/LinearCoolingSchedule.h" 16 | #include "ExplorationPlanner/kinematics/PlanarRobotVelCmdSamplerUniform.h" 17 | #include "ExplorationPlanner/kinematics/PlanarRobotVelCmdSamplerIndependentGaussian.h" 18 | #include "ExplorationPlanner/planner/SMCPlanner.h" 19 | 20 | // Frontier exploration services 21 | #include 22 | #include 23 | 24 | namespace explorationplanner_ros { 25 | 26 | ExplorationPlannerROS::ExplorationPlannerROS(const std::string name) 27 | : nh_(""), 28 | private_nh_("~"), 29 | tf_listener_(ros::Duration(10.0)), 30 | move_client_("move_base",true), 31 | explore_server_(nh_, name, boost::bind(&ExplorationPlannerROS::executeCb, this, _1), false), 32 | moving_(false), 33 | possibly_stuck_(false), 34 | goal_is_frontier_(false), 35 | global_map_(PlanarPose(0.0, 0.0, 0.0), 0.05) 36 | { 37 | // Read parameters and prepare the object for use accordingly 38 | readParameters(); 39 | 40 | // at this point, we can update trajectory evaluator (it uses only laser parameters) 41 | updateTrajectoryEvaluator(); 42 | 43 | // Subscribe to costmap from move_base and a global map from e.g. SLAM 44 | costmap_sub_ = nh_.subscribe("costmap", 1, &ExplorationPlannerROS::costmapCb, this); 45 | costmap_update_sub_ = nh_.subscribe("costmap_updates", 1, &ExplorationPlannerROS::costmapUpdateCb, this); 46 | 47 | global_map_sub_ = nh_.subscribe("map", 1, &ExplorationPlannerROS::globalmapCb, this); 48 | 49 | // dynamic reconfiguration setup 50 | confCb_ = boost::bind(&ExplorationPlannerROS::confCb, this, _1, _2); 51 | conf_server_.setCallback(confCb_); 52 | 53 | // Action server start 54 | explore_server_.registerPreemptCallback(boost::bind(&ExplorationPlannerROS::preemptCb, this)); 55 | explore_server_.start(); 56 | 57 | // paths inspected by the planner 58 | path_pub_ = nh_.advertise("planner_paths", 10, true); 59 | } 60 | 61 | void ExplorationPlannerROS::readParameters() 62 | { 63 | private_nh_.param("wait_between_planner_iterations", params_.wait_between_planner_iterations, false); 64 | 65 | private_nh_.param("lin_v_min", params_.lin_v_min, 0.3); 66 | private_nh_.param("lin_v_max", params_.lin_v_max, 1.0); 67 | private_nh_.param("ang_v_min", params_.ang_v_min, -1.0); 68 | private_nh_.param("ang_v_max", params_.ang_v_max, 1.0); 69 | private_nh_.param("f_rot_min", params_.f_rot_min, -0.02); 70 | private_nh_.param("f_rot_max", params_.f_rot_max, 0.02); 71 | 72 | private_nh_.param("min_traj_length", params_.min_traj_length, 0.5); 73 | private_nh_.param("min_reward", params_.min_reward, 50.0); 74 | private_nh_.param("max_sampling_tries", params_.max_sampling_tries, 30); 75 | 76 | private_nh_.param("laser_min_angle_deg", params_.laser_min_angle_deg, -90.0); 77 | private_nh_.param("laser_max_angle_deg", params_.laser_max_angle_deg, 90.0); 78 | private_nh_.param("laser_angle_step_deg", params_.laser_angle_step_deg, 5.0); 79 | private_nh_.param("laser_max_dist_m", params_.laser_max_dist_m, 4.0); 80 | private_nh_.param("p_false_pos", params_.p_false_pos, 0.05); 81 | private_nh_.param("p_false_neg", params_.p_false_neg, 0.05); 82 | 83 | private_nh_.param("horizon", params_.horizon, 3); 84 | private_nh_.param("discount", params_.discount, 1.0); 85 | 86 | private_nh_.param("schedule_a", params_.schedule_a, 3); 87 | private_nh_.param("schedule_b", params_.schedule_b, 3); 88 | private_nh_.param("num_kernels", params_.num_kernels, 5); 89 | 90 | private_nh_.param("num_particles", params_.num_particles, 10); 91 | ROS_ASSERT(params_.num_particles>0); 92 | private_nh_.param("resample_thresh", params_.resample_thresh, 0.33); 93 | 94 | private_nh_.param("std_vel", params_.std_vel, 0.2); 95 | private_nh_.param("std_ang", params_.std_ang, 0.1); 96 | private_nh_.param("std_fr", params_.std_fr, 0.02); 97 | private_nh_.param("default_ctrl_duration", params_.default_ctrl_duration, 1.0); 98 | 99 | private_nh_.param("allow_unknown_targets", params_.allow_unknown_targets, true); 100 | 101 | private_nh_.param("goal_execution_time", params_.goal_execution_time, 10.0); 102 | private_nh_.param("frontier_distance_threshold", params_.frontier_distance_threshold, 2.0); 103 | 104 | private_nh_.param("map_frame_id", params_.map_frame_id_, "map"); 105 | private_nh_.param("base_frame_id", params_.base_frame_id_, "base_link"); 106 | } 107 | 108 | void ExplorationPlannerROS::updateTrajectoryGenerator() 109 | { 110 | boost::mutex::scoped_lock lock(costmap_lock_); 111 | 112 | // Update the constraints 113 | RangeConstraint linvel_c(params_.lin_v_min, params_.lin_v_max); 114 | RangeConstraint angvel_c(params_.ang_v_min, params_.ang_v_max); 115 | RangeConstraint rot_c(params_.f_rot_min, params_.f_rot_max); 116 | VelocityPlanarKinematics kin(linvel_c, angvel_c, rot_c); 117 | 118 | // rebuild object with new trajectory checker 119 | boost::shared_ptr 120 | chk( new TrajectoryChecker(trajectorycheck_map_, params_.allow_unknown_targets)); 121 | 122 | // construct new trajectory generator object 123 | trajgenerator_ = boost::shared_ptr( 124 | new TrajectoryGenerator(kin, 125 | chk, 126 | params_.min_traj_length, 127 | params_.max_sampling_tries) 128 | ); 129 | } 130 | 131 | void ExplorationPlannerROS::updateTrajectoryEvaluator() 132 | { 133 | LaserScanner2D laser( params_.laser_min_angle_deg*(boost::math::constants::pi()/180), 134 | params_.laser_max_angle_deg*(boost::math::constants::pi()/180), 135 | params_.laser_angle_step_deg*(boost::math::constants::pi()/180), 136 | params_.laser_max_dist_m, 137 | params_.p_false_pos, 138 | params_.p_false_neg); 139 | trajevaluator_ = boost::shared_ptr( new TrajectoryEvaluator(laser) ); 140 | } 141 | 142 | 143 | std::unique_ptr ExplorationPlannerROS::buildPlanner() const 144 | { 145 | ROS_DEBUG("Building planner object..."); 146 | LinearCoolingSchedule schedule(params_.schedule_a, params_.schedule_b); 147 | SMCPlannerParameters param(schedule, params_.num_particles, params_.resample_thresh); 148 | 149 | PlanarRobotVelCmdSamplerUniform firstKC(params_.default_ctrl_duration); 150 | param.addKernel( firstKC ); 151 | for (unsigned int i = 0; i < params_.num_kernels; ++i) 152 | { 153 | const double ls = params_.std_vel / (i+1); 154 | const double as = params_.std_ang / (i+1); 155 | const double fs = params_.std_fr / (i+1); 156 | PlanarRobotVelCmdSamplerIndependentGaussian kg(ls, as, fs, params_.default_ctrl_duration); 157 | param.addKernel( kg ); 158 | } 159 | std::unique_ptr smc(new SMCPlanner(params_.horizon, params_.discount, trajgenerator_, trajevaluator_, param)); 160 | ROS_DEBUG("Planner object built"); 161 | return smc; 162 | } 163 | 164 | 165 | void ExplorationPlannerROS::confCb(ase_exploration::PlannerConfig &config, uint32_t level) 166 | { 167 | ROS_INFO("Reconfigure Request, level: %d", level); 168 | if (level == 0) 169 | { 170 | ROS_INFO("Planner parameters reconfigured"); 171 | params_.horizon = config.horizon; 172 | params_.discount = config.discount; 173 | params_.schedule_a = config.schedule_a; 174 | params_.schedule_b = config.schedule_b; 175 | params_.num_kernels = config.num_kernels; 176 | params_.std_vel = config.std_vel; 177 | params_.std_ang = config.std_ang; 178 | params_.std_fr = config.std_fr; 179 | params_.default_ctrl_duration = config.default_ctrl_duration; 180 | params_.num_particles = config.num_particles; 181 | params_.resample_thresh = config.resample_thresh; 182 | } 183 | else if (level == 1) 184 | { 185 | ROS_DEBUG("Sensor parameters reconfigured"); 186 | params_.laser_min_angle_deg = config.laser_min_angle_deg; 187 | params_.laser_max_angle_deg = config.laser_max_angle_deg; 188 | params_.laser_angle_step_deg = config.laser_angle_step_deg; 189 | params_.laser_max_dist_m = config.laser_max_dist_m; 190 | params_.p_false_pos = config.laser_p_false_pos; 191 | params_.p_false_neg = config.laser_p_false_neg; 192 | updateTrajectoryEvaluator(); 193 | } 194 | else if ( level == 2) 195 | { 196 | ROS_DEBUG("Trajectory generation parameters reconfigured"); 197 | params_.allow_unknown_targets = config.allow_unknown_targets; 198 | } 199 | else 200 | ROS_INFO("Undefined reconfiguration request"); 201 | } 202 | 203 | void ExplorationPlannerROS::executeCb(const ase_exploration::ExploreGoalConstPtr &goal) 204 | { 205 | ROS_INFO("Received new exploration task"); 206 | moving_ = false; 207 | possibly_stuck_ = false; 208 | goal_is_frontier_ = false; 209 | 210 | // wait for move_base to become available 211 | if(!move_client_.waitForServer() ) 212 | { 213 | explore_server_.setAborted(); 214 | return; 215 | } 216 | 217 | //loop until the task is finished or pre-empted 218 | while(ros::ok() && explore_server_.isActive()) 219 | { 220 | // get robot's current pose 221 | tf::StampedTransform tf_map_to_robot_base; 222 | try 223 | { 224 | tf_listener_.lookupTransform(params_.map_frame_id_, params_.base_frame_id_, 225 | ros::Time(0), tf_map_to_robot_base); 226 | } 227 | catch (tf::TransformException ex) 228 | { 229 | ROS_ERROR("TF Error: %s",ex.what()); 230 | ROS_ERROR("Exploration failed"); 231 | explore_server_.setAborted(); 232 | return; 233 | } 234 | PlanarPose robot_planar_pose(tf_map_to_robot_base.getOrigin().getX(), 235 | tf_map_to_robot_base.getOrigin().getY(), 236 | tf::getYaw(tf_map_to_robot_base.getRotation()) ); 237 | 238 | // Update the trajectory generator for this round 239 | updateTrajectoryGenerator(); 240 | std::unique_ptr planner = buildPlanner(); 241 | 242 | // get map information to use in planning 243 | global_map_lock_.lock(); 244 | ExplorationTaskState s(robot_planar_pose, global_map_ ); 245 | global_map_lock_.unlock(); 246 | 247 | // plan for requested number of steps 248 | std::vector avgRewards, minRewards, maxRewards, pathDist; 249 | unsigned int planner_iteration = 0; 250 | while ( (planner_iteration < params_.num_kernels) && explore_server_.isActive() ) 251 | { 252 | ros::Time t0 = ros::Time::now(); 253 | if ( !planner->iterate(s) ) 254 | ROS_WARN("Planner iteration failed"); 255 | ros::Time t1 = ros::Time::now(); 256 | ROS_INFO("Planner iteration %d complete in %f s", planner_iteration, (t1-t0).toSec()); 257 | 258 | // publish reward and path information 259 | ParticleSet ps = planner->getParticleSet(); 260 | std::vector rh = planner->getLatestRewardHandlers(); 261 | std::stringstream ss, sr, smin, smax; 262 | for (unsigned int i = 0; i < ps.getNumberOfParticles(); ++i) 263 | { 264 | std::vector path = ps[i].getTrajectory(); 265 | publishPath(path); 266 | 267 | ss << ps[i].getWeight() << ", "; 268 | smin << rh[i].getMinSumOfDiscRew() << ", "; 269 | smax << rh[i].getMaxSumOfDiscRew() << ", "; 270 | sr << rh[i].getAvgSumOfDiscRew() << ", "; 271 | 272 | // at last iteration, get summary information to decide what to do next 273 | if ( i == params_.num_kernels ) 274 | { 275 | avgRewards.push_back( rh[i].getAvgSumOfDiscRew() ); 276 | minRewards.push_back( rh[i].getMinSumOfDiscRew() ); 277 | maxRewards.push_back( rh[i].getMaxSumOfDiscRew() ); 278 | 279 | 280 | std::vector path = ps[i].getTrajectory(); 281 | PlanarPose target = path.end()[-2]; 282 | if ( path.size() == 2) // start pose + one other pose 283 | target = path.back(); 284 | 285 | double dx = robot_planar_pose.getX() - target.getX(); 286 | double dy = robot_planar_pose.getY() - target.getY(); 287 | pathDist.push_back( std::sqrt( dx*dx + dy*dy ) ); 288 | } 289 | } 290 | 291 | // Print particle information 292 | ROS_DEBUG("Particle weights: %s", ss.str().c_str()); 293 | ROS_DEBUG("Minimum discounted rewards: %s", smin.str().c_str()); 294 | ROS_DEBUG("Maximum discounted rewards: %s", smax.str().c_str()); 295 | ROS_DEBUG("Average discounted rewards: %s", sr.str().c_str()); 296 | 297 | if ( params_.wait_between_planner_iterations && (planner_iteration != params_.num_kernels) ) 298 | { 299 | ROS_INFO("Press button to continue"); 300 | std::cin.ignore(); 301 | } 302 | ++planner_iteration; 303 | } 304 | 305 | // get the best plan from the planner 306 | std::vector plan; 307 | planner->getPlan(plan); 308 | 309 | // heuristics for checking if we might be stuck 310 | if ( std::all_of( avgRewards.begin(), avgRewards.end(), [this](double r){return r < params_.min_reward;} ) || 311 | std::all_of( pathDist.begin(), pathDist.end(), [this](double d){return d < this->params_.min_traj_length; } ) 312 | ) 313 | { 314 | ROS_WARN("All rewards less than minimum of %f required or target closer than given threshold %f meters, planner may be stuck!", params_.min_reward, params_.min_traj_length); 315 | possibly_stuck_ = true; 316 | } 317 | 318 | 319 | // Construct the goal pose message to be filled and sent to move_base 320 | geometry_msgs::PoseStamped goal_pose; 321 | goal_pose.header.frame_id = params_.map_frame_id_; 322 | 323 | if (!possibly_stuck_) 324 | { 325 | // Publish whole path for best particle 326 | // Get the goal pose -- move to second to last pose on the path (if it exists) 327 | // end() is past the last element, -1 for last element, -2 for second-last 328 | PlanarPose target = plan.end()[-2]; 329 | if ( plan.size() == 2) // start pose + one other pose (horizon == 1) 330 | target = plan.back(); 331 | 332 | goal_pose.pose.position.x = target.getX(); 333 | goal_pose.pose.position.y = target.getY(); 334 | tf::Quaternion q; 335 | q.setRPY( 0, 0, target.getTheta() ); 336 | tf::quaternionTFToMsg(q, goal_pose.pose.orientation); 337 | 338 | // this goal is not a frontier 339 | goal_is_frontier_ = false; 340 | } 341 | else 342 | { 343 | // stuck - try to get a new target from frontier exploration 344 | ROS_INFO("Attempting recovery from stuck position via frontier exploration"); 345 | 346 | // create the costmap service clients 347 | ros::ServiceClient updateBoundaryPolygon = 348 | nh_.serviceClient 349 | ("frontier_exploration/explore_costmap/explore_boundary/update_boundary_polygon"); 350 | ros::ServiceClient getNextFrontier = 351 | nh_.serviceClient 352 | ("frontier_exploration/explore_costmap/explore_boundary/get_next_frontier"); 353 | 354 | if (!updateBoundaryPolygon.waitForExistence() || 355 | !getNextFrontier.waitForExistence() ) 356 | { 357 | ROS_ERROR("Services for frontier exploration not available; exploration task canceled"); 358 | explore_server_.setAborted(); 359 | return; 360 | } 361 | 362 | // set unbounded for frontier_exploration 363 | frontier_exploration::UpdateBoundaryPolygon bndsrv; 364 | bndsrv.request.explore_boundary.header.frame_id = params_.map_frame_id_; 365 | if(updateBoundaryPolygon.call(bndsrv)) 366 | { 367 | ROS_DEBUG("Region boundary set for frontier_exploration"); 368 | } 369 | else 370 | { 371 | ROS_ERROR("Failed to set region boundary for frontier_exploration"); 372 | explore_server_.setAborted(); 373 | return; 374 | } 375 | 376 | // Service call to frontier_exploration 377 | frontier_exploration::GetNextFrontier srv; 378 | // give current pose as start for frontier search 379 | srv.request.start_pose.header.frame_id = params_.map_frame_id_; 380 | srv.request.start_pose.pose.position.x = robot_planar_pose.getX(); 381 | srv.request.start_pose.pose.position.y = robot_planar_pose.getY(); 382 | tf::quaternionTFToMsg(tf_map_to_robot_base.getRotation(), srv.request.start_pose.pose.orientation); 383 | 384 | if ( getNextFrontier.call(srv) ) 385 | { 386 | ROS_INFO("Got next frontier from frontier_exploration"); 387 | goal_pose = srv.response.next_frontier; 388 | } 389 | else 390 | { 391 | ROS_ERROR("Call to service %s failed", getNextFrontier.getService().c_str() ); 392 | explore_server_.setAborted(); 393 | break; 394 | } 395 | // this goal is a frontier 396 | goal_is_frontier_ = true; 397 | possibly_stuck_ = false; 398 | } 399 | 400 | // pass goal to move_base 401 | feedback_.current_target = goal_pose; 402 | explore_server_.publishFeedback(feedback_); 403 | 404 | move_client_goal_.target_pose = goal_pose; 405 | boost::unique_lock lock(move_client_lock_); 406 | goal_time_ = ros::Time::now(); 407 | if (explore_server_.isActive()) 408 | { 409 | move_client_.sendGoal(move_client_goal_, 410 | boost::bind(&ExplorationPlannerROS::doneMovingCb, this, _1, _2), 411 | 0, 412 | boost::bind(&ExplorationPlannerROS::feedbackMovingCb, this, _1)); 413 | moving_ = true; 414 | } 415 | lock.unlock(); 416 | 417 | //wait for movement to finish before continuing 418 | while(ros::ok() && explore_server_.isActive() && moving_) 419 | { 420 | ros::WallDuration(0.1).sleep(); 421 | } 422 | } 423 | 424 | ROS_ASSERT(!explore_server_.isActive()); 425 | } 426 | 427 | void ExplorationPlannerROS::publishPath(const std::vector& p) 428 | { 429 | nav_msgs::Path pm; 430 | pm.header.frame_id = params_.map_frame_id_; 431 | unsigned int id = 0; 432 | for (std::vector::const_iterator it = p.begin(), iend = p.end(); it != iend; ++it) 433 | { 434 | geometry_msgs::PoseStamped ps; 435 | ps.header.frame_id = pm.header.frame_id; 436 | ps.header.stamp = ros::Time::now(); 437 | ps.header.seq = id++; 438 | ps.pose.position.x = it->getX(); 439 | ps.pose.position.y = it->getY(); 440 | ps.pose.orientation.w = 1.0; 441 | pm.poses.push_back(ps); 442 | } 443 | 444 | path_pub_.publish(pm); 445 | } 446 | 447 | void ExplorationPlannerROS::preemptCb() 448 | { 449 | boost::unique_lock lock(move_client_lock_); 450 | move_client_.cancelGoalsAtAndBeforeTime(ros::Time::now()); 451 | ROS_WARN("Current exploration task cancelled"); 452 | 453 | if(explore_server_.isActive()){ 454 | explore_server_.setPreempted(); 455 | } 456 | } 457 | 458 | void ExplorationPlannerROS::doneMovingCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result){ 459 | 460 | if (state == actionlib::SimpleClientGoalState::ABORTED) 461 | { 462 | ROS_ERROR("Failed to move"); 463 | explore_server_.setAborted(); 464 | } 465 | else if (state == actionlib::SimpleClientGoalState::SUCCEEDED) 466 | { 467 | moving_ = false; 468 | } 469 | } 470 | 471 | void ExplorationPlannerROS::feedbackMovingCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback) 472 | { 473 | ROS_DEBUG("move_base feedback received"); 474 | bool cancel_goal = false; 475 | 476 | // if this is "regular goal" or frontier goal, check if we already executed it for given execution time 477 | double time_spent_on_goal = (ros::Time::now() - goal_time_).toSec(); 478 | if ( !goal_is_frontier_ && time_spent_on_goal > params_.goal_execution_time) 479 | { 480 | ROS_INFO("Current exploration task ran for execution time of %f seconds, cancel it and replan", params_.goal_execution_time); 481 | cancel_goal = true; 482 | } 483 | 484 | // if this goal is a frontier, check if we are "close" to it and can move to regular planning again 485 | double dx = move_client_goal_.target_pose.pose.position.x - feedback->base_position.pose.position.x; 486 | double dy = move_client_goal_.target_pose.pose.position.y - feedback->base_position.pose.position.y; 487 | double distance_to_target = std::sqrt( dx*dx + dy*dy ); 488 | if ( goal_is_frontier_ && distance_to_target < params_.frontier_distance_threshold ) 489 | { 490 | ROS_INFO("Current frontier closer than %f meters, cancel it and replan", params_.frontier_distance_threshold); 491 | cancel_goal = true; 492 | } 493 | 494 | if ( cancel_goal ) 495 | { 496 | boost::unique_lock lock(move_client_lock_); 497 | move_client_.cancelGoalsAtAndBeforeTime(ros::Time::now()); 498 | moving_ = false; 499 | } 500 | 501 | } 502 | 503 | void ExplorationPlannerROS::costmapCb(const nav_msgs::OccupancyGrid& map) 504 | { 505 | boost::mutex::scoped_lock lock(costmap_lock_); 506 | 507 | if (params_.map_frame_id_ != map.header.frame_id) 508 | { 509 | ROS_ERROR("Incoming costmap on topic %s has frame %s, expected %s. Costmap not updated.", 510 | costmap_sub_.getTopic().c_str(), 511 | map.header.frame_id.c_str(), 512 | params_.map_frame_id_.c_str()); 513 | return; 514 | } 515 | 516 | // sparsify the costmap. 517 | // update instead of rewrite might be more efficient, but rewrite fast enough for now... 518 | PlanarPose map_origin(map.info.origin.position.x, 519 | map.info.origin.position.y, 520 | tf::getYaw(map.info.origin.orientation)); 521 | trajectorycheck_map_ = boost::shared_ptr( 522 | new PlanarGridBinaryMap(map_origin, map.info.resolution)); 523 | 524 | for (unsigned int h = 0; h < map.info.height; ++h) 525 | { 526 | const unsigned int h_offset = h * map.info.width; 527 | for (unsigned int w = 0; w < map.info.width; ++w) 528 | { 529 | // in ROS costmap_2d, costmap_2d::INSCRIBED obstacle corresponds 530 | // to occupancy 99. We set our threshold there. 531 | if ( map.data[h_offset + w] != -1 && 532 | map.data[h_offset + w] >= 99 ) 533 | { 534 | trajectorycheck_map_->setGridValue( PlanarGridIndex(w,h), true); 535 | } 536 | } 537 | } 538 | } 539 | 540 | void ExplorationPlannerROS::costmapUpdateCb(const map_msgs::OccupancyGridUpdate &map_update) 541 | { 542 | boost::mutex::scoped_lock lock(costmap_lock_); 543 | 544 | if (params_.map_frame_id_ != map_update.header.frame_id) 545 | { 546 | ROS_ERROR("Incoming costmap on topic %s has frame %s, expected %s. Costmap not updated.", 547 | costmap_sub_.getTopic().c_str(), 548 | map_update.header.frame_id.c_str(), 549 | params_.map_frame_id_.c_str()); 550 | return; 551 | } 552 | 553 | for (unsigned int h = 0; h < map_update.height; ++h) 554 | { 555 | const unsigned int h_offset = h * map_update.width; 556 | for (unsigned int w = 0; w < map_update.width; ++w) 557 | { 558 | // in ROS costmap_2d, costmap_2d::INSCRIBED obstacle corresponds 559 | // to occupancy 99. We set our threshold there. 560 | if ( map_update.data[h_offset + w] != -1 && 561 | map_update.data[h_offset + w] >= 99 ) 562 | { 563 | trajectorycheck_map_->setGridValue( PlanarGridIndex( map_update.x + w, map_update.y + h), true); 564 | } 565 | } 566 | } 567 | } 568 | 569 | 570 | void ExplorationPlannerROS::globalmapCb(const nav_msgs::OccupancyGrid &map) 571 | { 572 | boost::mutex::scoped_lock lock(global_map_lock_); 573 | 574 | if (params_.map_frame_id_ != map.header.frame_id) 575 | { 576 | ROS_ERROR("Incoming global map on topic %s has frame id: %s, expected frame id: %s. Global map not updated.", 577 | global_map_sub_.getTopic().c_str(), 578 | map.header.frame_id.c_str(), 579 | params_.map_frame_id_.c_str()); 580 | return; 581 | } 582 | 583 | // sparsify the incoming global map 584 | PlanarPose map_origin(map.info.origin.position.x, 585 | map.info.origin.position.y, 586 | tf::getYaw(map.info.origin.orientation)); 587 | global_map_ = PlanarGridOccupancyMap(map_origin, map.info.resolution); 588 | for (unsigned int h = 0; h < map.info.height; ++h) 589 | { 590 | const unsigned int h_offset = h*map.info.width; 591 | for (unsigned int w = 0; w < map.info.width; ++w) 592 | { 593 | if ( map.data[h_offset+w] != -1) // -1 implicitly unknown 594 | global_map_.setGridValue( PlanarGridIndex(w,h), map.data[h_offset+w]); 595 | } 596 | } 597 | } 598 | 599 | } // namespace explorationplanner_ros 600 | -------------------------------------------------------------------------------- /src/exploration_planner_node.cpp: -------------------------------------------------------------------------------- 1 | #include "ExplorationPlannerROS/ExplorationPlannerROS.h" 2 | #include 3 | 4 | int main(int argc, char **argv) 5 | { 6 | ros::init(argc, argv, "ase_exploration_planner_node"); 7 | ROS_INFO("Started node exploration_planner_node"); 8 | explorationplanner_ros::ExplorationPlannerROS planner("exploration"); 9 | 10 | ros::spin(); 11 | return 0; 12 | } 13 | --------------------------------------------------------------------------------