[#408] rviz and launch integration with environment for testing and tuning closed-loop behavior

Description

Updated package controller_testing to include

• added more parameterization, by converting hardcore values, like stop and report time
• ability to disable, trajectory generation, to use external trajectory generation.
• realtime mode (not faster than realtime)
• publishing tf and Odometry
• x-y history plot, with rainbow color progressing with time (can detect reverse movement)

Updated package test_trajectory_following to include

• support for sim_type=dynamics uses controller_testing_node, can run faster than realtime
• support for sim_type=kinematics uses vehicle_kinematics_sim.py
• default sim is sim_type=dynamics, for lgsvl use sim_type=lgsvl, in
  • simple_trajectory_following.launch.py and
  • trajectory_spoofer_mpc_control.launch.py

Notes for Reviewer

Update params in src/motion/control/controller_testing/param/defaults.param.yaml

ros2 launch controller_testing controller_testing_node.launch.py real_time_sim:=True with_rviz:=True

To test following a circle

Update params in src/motion/control/controller_testing/param/defaults.param.yaml

ros2 launch test_trajectory_following trajectory_spoofer_mpc_control.launch.py sim_type:=dynamics

Checklist

• Documentation was updated

/milestone %After AVP