Implement MotionControl

[Sharwin24]

With the ultimate goal of a GoToGoal command, there are a lot of potential solutions and various TODOs to accomplish. Here they are in no particular order:

• Start designing a controller for trajectory tracking (PID, MPC, LQR, MPPI)
  • Implement a simple Pure Pursuit controller for testing
• Ackermann Steering Planner for translating Twist commands to wheel velocities and steering angles
• Utilize AckermannDrive messages for sending wheel commands around ROS ecosystem
• Create a ROS node to handle motion control logic:
  • Subscribe to goal pose and current robot state
  • Plan a wheel velocity and steering angle trajectory
  • Publish wheel commands to the appropriate topic (later be consumed by comms node for sending to MCU)