Parking trajectory for diff-drive robot

[soblin]

Hi,

I am using autoware for the development of mobile robot of differential drive type for indoor application, but I've found out that in executing return-to-home behavior (like returning to charging spot) to the parking lane (we already have the lanelet map, and placed parking spot there) the trajectory resembles the one of car-like vehicle and includes turning points. We also tried to decrease wheel_base parameter to lower the turning radius of that trajectory, but still we cannot avoid them. Of course we recognize that the main scope of autoware is car-like vehicle, but we would be happy if we could let our robot approach the destination as close as possible and then make a turn there.

Can you educate us on how to customize the (parking) trajectory generation module by any chance ?

[takayuki5168]

Here we use Hybrid A* search for parking planning.
https://github.com/autowarefoundation/autoware.universe/tree/main/planning/freespace_planning_algorithms/src

In Hybrid A* search, we assume that the vehicle's following path as reeds shepp curves. That's why the generated trajectory is like car-like vehicle's one.
To handle diff drive robot, we have to add its dynamics or kinematics model instead of reeds shepp in Hybrid A* search.

[takayuki5168]

We will have a big refactoring and improvement on freespace planning by @HiroIshida, so please keep in touch if you modify the package.

[HiroIshida]

@soblin
I think the problem is two fold.

First, and most importantly, the transition table that determines the astar's node transition is hard-coded to "car-like" vehicle kinematics. So, the resulting trajectory is too conventional for diff-drive robot.
https://github.com/autowarefoundation/autoware.universe/blob/bb96ae7b56085a6054ad1ade970bad399faef35a/planning/freespace_planning_algorithms/src/astar_search.cpp#L74
You can replace this transition table by the one of diff-drive robot, to make trajectory more suitable for the diff-drive robot. I personally think possible modification would be creating an abstract class for discrete kinematics, and diff-drive and car-like kinematics class as subclass of the abstract class. Then, creating a transition table depending on specified kinematics would be clean solution of the problem.

Second one is the one mentioned by @takayuki5168. Even after you replace the transition table with the diff-drive robot's one, because the reeds-shepp distance is used as a heuristic function, "car-like" path will be preferred. https://github.com/autowarefoundation/autoware.universe/blob/bb96ae7b56085a6054ad1ade970bad399faef35a/planning/freespace_planning_algorithms/src/astar_search.cpp#L214
Originally, euclidean heuristic function is used in astar planning, but it was deleted before because reeds-shepp is far better for car-like robot. Possible change would be adding a rosparam to select the heuristic function.

I will not make a "big change" on freespace planning algorithm package but I am going to open a PR that adds RRT* and informed-RRT* to the package in few days. It would be really helpfule If your PR on this problem is after the merge of my PR on RRT*.