If you're okay with non-optimal paths, I recommend using the Open Motion Planning Library. It contains implementations of several sampling-based planning algorithms (see https://ompl.kavrakilab.org/planners.html). Many of OMPL's algorithms produce optimal solutions, but given the non-holonomic nature of the tractor-trailer system, it will be difficult to use them. I recommend using the RRT algorithm described in the above link under the control-based planners section. OMPL does not come with collision-detection, so you'll have to implement that on your own.
If you want optimal paths, you have a few options:
Option A: Implement your own version of the sbpl cart planner. The sbpl cart planner relies on a set of pre-defined motion primitives to incrementally construct and search a lattice graph. See https://www.cs.cmu.edu/~maxim/files/tutorials/robschooltutorial_oct10.pdf and https://github.com/AtsushiSakai/HybridAStarTrailer for more details.
Option B: Use a near-optimal sampling-based planning algorithm from OMPL's suite of control-based planners. I recommend the SST algorithm.
Option C: Use an asymptotically optimal planner from OMPL's suite of geometric planners. I recommend RRT*. In order to use this, you'll need to define an inverse kinematic function that can connect any two states by a kinematically feasible path. For instructions on how to do this, read http://www.cs.uu.nl/research/techreps/repo/CS-1996/1996-09.pdf.
Among these options, I recommend Option A. Lattice planning is not too difficult to implement and generally gives good results in practice.
Originally posted by DubinsCar with karma: 36 on 2021-01-13
This answer was ACCEPTED on the original site
Post score: 2
Original comments
Comment by hanks on 2021-01-13:
Thank you for your reply. I was aware of the Hybrid A Star Trailer project by Atsushi Sakai and wanted to implement something similar. I will look into all these options thoroughly starting with option A.
Also, if you could suggest any SLAM and localization packages/algorithms that can be an improvement over Hector SLAM and AMCL which I'm currently using. I am looking into mapping algorithms (RTLAB, ORB SLAM, etc) as Hector SLAM isn't able to construct (even with odometry enabled) the long hallways I have in my environment. I'd appreciate your suggestions, and thank you for the detailed answer.
Comment by JackB on 2021-01-15:
@tanujthakkar in my personal experience I have found gmapping to be pretty robust and very usable in the ROS ecosystem for a SLAM solution.
Comment by hanks on 2021-01-15:
I tried using GMapping but was experiencing a lot of odometry drift. Anyway, I solved the problem I had with Hector SLAM but I'll give GMapping another shot if it works better. Thanks for the reply.
Comment by hanks on 2021-03-05:
@DubinsCar I am currently implementing the Hybrid A* algorithm in C++ for ROS but am unable to figure out how to implement collision check for the robot and the trailer. Do I just check the entire robot and trailer polygon with the respective map data for collisions? Or is there a better way to implement it? Please help.
Comment by DubinsCar on 2021-03-08:
I personally like using the approach described in http://lavalle.pl/planning/node211.html, specifically sections 5.3.2 and 5.3.4. This approach is computationally efficient and works for arbitrarily complex robot and obstacle shapes. If your map data is a point cloud, then you can replace the polygon tree described in section 5.3.2 with a kd-tree or ball-tree.