toanhd93 / ma_rrt_path_plan

RRT with multiple remote goals.

This repository contains the ROS package with code implementation of RRT-based path planning algorithm suitable for exploration of a trackdrive circuit according to the rules of Formula Student Driverless competition in Germany. This algorithm is based on my contribution to E-gnition, a FS Team of Technical University Hamburg.

A brief introduction to the main steps of the proposed algorithm is given in my Master's thesis presentation (direct timestamp).

Notes

• The algorithm does not utilize the cones' color information, but instead a simple logic is used, which rewards the branches with cones from both sides (see findBestBranch(...) method), and penalizes the branches having cones only from one side. With cone classification information a better rating system can be implemented and applied.
• Unfortunately I wasn't able to test and see this algorithm working on real hardware, a FS Driverless car, so I am excited if you can bring it the reality on any mobile robot and share some videos with me (see section Usage)

Usage

• Exploration of FSG'18 trackdrive circuit in Gazebo.
• Exploration of FSG'17 trackdrive circuit in Gazebo.
• Your video (feel free to pull-request a link with it here).

Inputs, outputs, params

Inputs

• rospy.Subscriber("/map", Map, ...)
• rospy.Subscriber("/odometry", Odometry, ...)

Outputs

• rospy.Publisher("/waypoints", WaypointsArray, ...)

Outputs (Visuals)

• rospy.Publisher("/visual/tree_marker_array", MarkerArray, ...)
• rospy.Publisher("/visual/best_tree_branch", Marker, ...)
• rospy.Publisher("/visual/filtered_tree_branch", Marker, ...)
• rospy.Publisher("/visual/delaunay_lines", Marker, ...)
• rospy.Publisher("/visual/waypoints", MarkerArray, ...)

Parameters to tune (main)

• planDistance = 12 m - Maximum length of tree branch
• expandDistance = 1 m - Length of tree node/step
• expandAngle = 20 deg - constraining angle for next tree nodes
• coneObstacleSize = 1.1 m - size of obstacles derived from cone position
• coneTargetsDistRatio = 0.5 - ratio to frontConesDist for deriving remote cone goals

Licence

MIT

• Use me and my code in any way you want
• Keep the names and the same licence