planning sometimes fails when using RRT* for parking

[TakaHoribe]

Checklist

• [X] I've read the contribution guidelines.
• [X] I've searched other issues and no duplicate issues were found.
• [X] I'm convinced that this is not my fault but a bug.

Description

planning fails when driving in parking lots with rrtstar algorithm in the freespace_planner.

The solved trajectory by the freespace_planner is visualized as in the picture below, but the output from the motion_velocity_smoother is not visualized.

Attach the terminal output for further information.

Expected behavior

arrive at a goal with rrtstar algorithm in parking scenario.

Actual behavior

planning fails and it won't arrive at a goal

Steps to reproduce

1. change the default param in freespace_planner from astar to rrtstar
2. set a goal to in the parking lot and run in the parking scenario

Versions

No response

Possible causes

No response

Additional context

No response

[TakaHoribe]

no one is assigned now

[shulanbushangshu]

I test the "rrtstar" in the parking.The path display planning is successful, but the trajectory of motion_velocity_smoother display wrong is in the process of control. Simulation of the video as follows:

https://user-images.githubusercontent.com/102840938/201040744-436d0277-f2e3-4782-9256-b000b19c9f85.mp4

[NorahXiong]

It is caused by driving direction change point error. I will fix it.