Lab 3 - Visibility Graph Path Planning
In this lab, you are required to implement visibility graph (vgraph) path planning algorithm using the ArbotiX turtlebot simulator and visualize the graph and robot path following in RViz.
This repo is a ROS package that you can add to your own catkin workspace to create a map with obstacles (
map.yaml) and load it in RViz. It allows you to custom your map with any shape of polygon obstacles. All length unit we use is in cm.
The package is tested on
Ubuntu 14.04 with
OpenCV 3.1.0 and
numpy 1.15.1. You should also have the ros-by-example code already installed.
data/world_obstacles.txt: Text file indicating obstacle locations (in cm).
- First line is the total number of obstacles in the world.
- Each obstacle starts with a number specifying total number of vertices, followed by these vertices' location in counter-clockwise order.
data/goal.txt: Location of the goal (in cm).
src/create_map.py: Creates or updates the
maps/folder, using obatacles and goal defined in the above files.
launch/launch.launch: Launch file to start everything you need.
maps/map.png: Image of the map.
maps/map.yaml: Map metadata.
package.xml: Package metadata.
world.rviz: RViz configuration file.
The map we use in RViz is 1200cm by 600cm, top left and bottom right are (-300, 300) and (900, -300) respectively. Also note that cells in the RViz grid are 50cm by 50cm. The start position of the robot is always at (0, 0), facing x-positive and the goal is defined in
data/goal.txt. The obstacles are orange polygons and the goal is a purple dot. More about using the
map_server package here.
Clone this repo and compile it in your catkin workspace
cd ~/catkin_ws/src git clone https://github.com/jingxixu/vgraph.git cd ../ catkin_make source devel/setup.bash rospack profile
Then, start everything
roslaunch vgraph launch.launch
Remeber to start a new terminal if you want to run other commands after this.
To custom obstacles and goal position, modify
/data/goal.txt, then run the following commands. It will update the
map.png image file loaded by
roslaunch. You will need to re-launch everything to visualize the modified map in RViz.
roscd vgraph/src python create_map.py
Instructions and Rubric
You should add python files in
/src to do this lab. The visualization should be done using visualization_msgs/Marker or visualization_msgs/MarkerArray message (more about using RViz markers here). We expect to see the following results in your demo video and from running your programs.
- Bring up ArbotiX turtlebot simulator and RViz with the map of obstacles and goal we give you. (20 points)
- Grow the obstacles using reflection algorithm (flip the robot around the reference point, place it at each vertex of the obstacle, and create a convex hull). You can assume the robot to be a 36cm-by-36cm square. You can pick the reference point anyware on this square. Given a list of 2d points, you are allowed to use any existing library/package to return a convex hull. My recommendation is scipy.spatial.ConvexHull. The following example picks the center as the reference point. (20 points)
- Create the vgraph by first fully connecting all obstacle vertices + start + goal and then implement your own collision checker to remove edges that collides with obstacles (except endpoints). You are NOT allowed to use any existing libraries/packages for collision checking and the only package we expect to see in your collision checker is
numpy. (20 points)
- Implement your own shortest path finding algorithm to find a shortest path from start to goal. Same as collision checker, you are NOT allowed to use any existing package/library. (20 points)
- Modify your robot controller from Lab 1 and 2 to make the robot follow the path. It is allowed to run this part in a separate program by copying/pasting the returned list of waypoints and feed into your follow-path function. (20 points)
- You should submit a
lab3_UNI1_UNI2.tar.gzfile which contains the modified package
vgraphthat you cloned.
- It should include all files that we need to reproduce your video demo.
- Remeber to delete the
imgs/folder as we do not need it and it is huge.
- You should replace everything in the existing
README.mdwith the following content:
- Usage: how to run your code to reproduce your video demo. Clearly explain the functionalities of all added scripts.
- Method: a brief description of your methods.
- Video: a link to the Youtube video of a working demo
- Others: anything else you would like to include
- Violation of these submission instructions will result in point deduction.