Add safety state checks

[alyashour]

A recent endeavor into a nan bug has highlighted the need to do state checks in control in the case it's being fed garbage data.

This change involves doing range checks on these 4 state vars (and any others that can be found):
ap1_msgs::msg::SpeedProfileStamped::SharedPtr speed_profile_;
ap1_msgs::msg::TargetPathStamped::SharedPtr target_path_;
ap1_msgs::msg::FloatStamped::SharedPtr vehicle_speed_;
ap1_msgs::msg::FloatStamped::SharedPtr vehicle_turn_angle;

If they are nan, inf, or negative when they should be positive the car should flag it as dangerous and communicate that up to emergency stop.

This likely also means we need to replace the ::SharedPtr versions of the memory with locally owned floats and vectors (since null is technically a valid state now. In the future this shouldn't be allowed).