Parking Planner and MPC are Operating in Different Frames
Description
Currently, LGSVL produces odometry data in the base_link
frame. This pose information is used to populate the VehicleKinematicState
messages which are consumed by the MPC controller and the Behavior Planner. The problem is that the MPC controller and Parking Planner plan using the vehicle's Center of Gravity to target the next trajectory point. Since the vehicle ego position estimates are not in the same frame as the MPC planner is planning, you have to introduce a hack (which is currently implemented in the Parking Planner parameters but not the MPC Controller parameters) to move the Center of Gravity over the origin of base_link
for planning and control to work correctly.
How to Reproduce
Use the MS3 launch file to plan a Reverse Park.
Current Behavior
Due to the precise positioning necessary for a reverse parking maneuver, the offset can be seen in the vehicle cutting planned turns too sharply and not being oriented correctly when it comes to a stop in the parking spot.
Expected behavior
Planning and control should both use the Center of Gravity, as designed.