Added visual debug topics in record replay planner

Description

Integration of record replay planner with obstacle detection stack from ms2.launch in trajectory_recording.launch.py of package test_trajectory_following.

Additionally, some helpful debug topics added has been added to record replay planner to verify correct functionality and visualize how collision detection is working. This is also very useful for troubleshooting if collision detection is not working properly.

Notes for Reviewer

Relates to #181 (closed). record_replay_with_obstacle_mpc_control_lgsvl

Usage:

Run LGSVL simulator, as per howto.

ros2 launch test_trajectory_following trajectory_recording.launch.py

Updated docs README

• on Joystick press start button to record
• drive vehicle manually in simulator/real world (not using the joystick)
• press back button to stop recording
• reset position of vehicle manually before replaying (may restart simulation)
• press logo/home button to start replay
  • If with_mpc:=True Let MPC drive or else with_mpc:=False drive vehicle manually in LGSVL
  • If with_obstacle:=True trajectory replay should obstacles in simulation (perception stack also launched).
• press back button to stop replaying

You can also skip recording and use this file instead recorded_trajectory.data, with AutonomousStaff Map, with default starting position.

Checklist

• Documentation was updated
  • Usage updated in test_trajectory_following/README
  • Design in recordreplay_planner_node

/milestone %After AVP