A Probabilistic RoadMap (PRM) simulator in ROS.
Planning a path from the robot's position to multiple goal locations.
A probabilistic roadmap planner is a common motion-planning algorithm used in robotics. In taking random samples from the free space surrounding a robot, the algorithm attempts to connect configurations (groups of samples) to capture the connectivity of the overall environment. Shortest path finding algorithms (such as Dijkstra) may then be used to find a path between a start and end goal.
This implementation uses LD-PRM planning. The idea behind LD-PRM is to capture the connectivity of the configuration space with fewer samples, reducing the overall running time of the algorithm. Different to a simple PRM planner, the samples generated by the LD-PRM must fulfil an important criterion in order to be included on the roadmap. This constraint can be thought of as a 'separation distance' - new samples are forbidden to be within a certain distance of other samples already on the roadmap.
This criterion helps create an almost uniform distribution of samples across the free configuration space. As the PRM network increases in size, this separation distance decreases, and the network better captures the entire form of the known environment. This also helps the planner navigate narrow passages, which present significant challenges in other PRM implementations.
Khaksar, W. Hong, T. Khaksar, M. Motlagh, O. 2013, "A Low Dispersion Probabilistic Roadmaps (LD-PRM) Algorithm for Fast and Efficient Sampling-Based Motion Planning", International Journal of Advanced Robotic Systems, Vol.10, Link
The Probabilistic Roadmap simulator has been tested on Ubuntu 14.04 / ROS Indigo and Ubuntu 16.04 / ROS Kinetic.
ROS Indigo:
$ sudo apt-get install ros-indigo-local-map ros-indigo-stage-ros ros-indigo-rosdoc-liteROS Kinetic:
$ sudo apt-get install ros-kinetic-stage-ros ros-kinetic-rosdoc-liteFor Kinetic users, they must install local_map from source as it is not part of the standard kinetic repositories. You can find the docs and source location for local_map here.
This project uses catkin and CMake to build it. After placing the project in the src directory of the catkin workspace catkin_ws/src/prm_sim, run catkin_make -j1 (in the catkin workspace) to auto-generate src from the srv file.
After that initial build, calls to catkin_make will suffice.
This project also comes with a series of unit tests that are run on a gtest framework. To build these tests run catkin_make tests from the catkin workspace. To run these tests simply execute the following in catkin_ws:
$ ./devel/lib/prm_sim/prm_sim-testIt is important to note that due to the non-deterministic behaviour of prm network building, there is a chance some of the unit tests could fail. To visualise the generation of the prm networks during unit testing, pass --show as a command line argument. To increase or decrease the amount of prm building rounds (default is ten) during unit testing, pass -t <max_rounds> as a command line argument.
For example if we wanted to show the building of the prm network for 3 rounds at most, we would run:
$ ./devel/lib/prm_sim/prm_sim-test --show -t 3
Sample output of tests.
First and foremost, roscore must be started to service all the various ros nodes:
$ roscore
...Next, start the world stage which will allow you to click and drag the position of the robot within the map:
$ rosrun stage_ros stageros $(rospack find prm_sim)/worlds/uoa_robotics_lab.world
[ INFO] [1508799387.191771549]: Found 2 laser devices and 0 cameras in robot 0
[ WARN] [1508799387.298406431]: ROS Stage currently supports rangers with 1 sensor only.
...Next, start the local_map node, which converts the world map above into an ogMap:
$ rosrun local_map local_map /local_map/scan:=/base_scan_1 _map_width:=200 _map_height:=200 _map_resolution:=0.1
[ INFO] [1508799528.068394858, 0.200000000]: Found world frame odom
...Next start the map_to_image node which coverts ogMaps to openCV images:
$ rosrun prm_sim prm_sim_image_node map:=/local_map/local_map /pose:=/odom
[ INFO] [1508799607.900065056]: Map to Image node started.
...Finally, and most importantly - start the prm simulator by running:
$ rosrun prm_sim prm_sim_node
[ INFO] [1508799676.104498061]: Init with: map_size={20.0} resolution={0.1} robot_diameter={0.2} density={5}
...If one wishes to run the simulator with default values, the start.sh script will execute all the above commands for convenience.
One might want to additionally visualise the produced prm network and the robot's configuration space. To do so, the following commands can be additionally run.
Visualisation of robot pose, map space and line of sight:
$ rosrun rviz rviz -d $(rospack find prm_sim)/rviz/pfms.rvizVisualisation of the prm network and path (looking at topic /prm):
$ rqt_image_viewOnce all the relevant ROS nodes have been started, one can start requesting goals for the simulator to plan a path towards. This achieved by posting messages on the ros service /request_goal. For example if I wanted to plan a path between the robot's current position and the map coordinates (x=2.7, y=3.1), I would execute:
$ rosservice call /request_goal -- 2.7 3.1When requesting a goal, x and y coordinates are rounded to one decimal place. Note that goals outside of the configuration space, or in unknown areas, will be dumped. To see if a path was successfully planned, check the logs produced by prm_sim_node. For example, a successful path plan may look like this:
[ INFO] [1508800366.505862595, 22.300000000]: Goal request: x=2.7, y=3.1
[ INFO] [1508800366.505907803, 22.300000000]: Sending back goal response: [1]
[ INFO] [1508800366.506000994, 22.300000000]: Setting reference: {0.0, 0.0}
[ INFO] [1508800366.586393333, 22.400000000]: Starting build: {0.0, 0.0} to {2.7, 3.1}
[ INFO] [1508800366.586428938, 22.400000000]: Building nodes...
[ INFO] [1508800366.648999653, 22.400000000]: Sent path information...
[ INFO] [1508800366.649113146, 22.400000000]: Updating PRM overlay...