Merge pull request #342 from tier4/fix/ros2_input_nan_values

Fix VehicleRos2Input causing SilentError in binary AWSIM
tier4 · Aug 7, 2024 · 0ca4970 · 0ca4970
2 parents acffcba + db889e4
commit 0ca4970
Showing 1 changed file with 29 additions and 3 deletions.
diff --git a/Assets/AWSIM/Scripts/Vehicles/VehicleRos2Input.cs b/Assets/AWSIM/Scripts/Vehicles/VehicleRos2Input.cs
@@ -1,3 +1,4 @@
+using System;
 using System.Collections;
 using System.Collections.Generic;
 using UnityEngine;
@@ -93,10 +94,12 @@ void OnEnable()
                     {
                         // highest priority is EMERGENCY.
                         // If Emergency is true, ControlCommand is not used for vehicle acceleration input.
-                        if (!isEmergency)
-                            AccelerationInput = msg.Longitudinal.Acceleration;
+                        if (isEmergency)
+                        {
+                            return;
+                        }
 
-                        SteeringInput = -(float)msg.Lateral.Steering_tire_angle * Mathf.Rad2Deg;
+                        ValidateAndSetVehicleCommand(msg);
                     }, qos);
 
             gearCommandSubscriber
@@ -118,6 +121,29 @@ void OnEnable()
                     });
         }
 
+        void ValidateAndSetVehicleCommand(autoware_auto_control_msgs.msg.AckermannControlCommand command)
+        {
+            if (Single.IsNaN(command.Longitudinal.Acceleration))
+            {
+                Debug.LogError($"AccelerationInput NaN. Setting EmergencyDeceleration");
+                AccelerationInput = emergencyDeceleration;
+            }
+            else
+            {
+                AccelerationInput = command.Longitudinal.Acceleration;
+            }
+
+            if (Single.IsNaN(command.Lateral.Steering_tire_angle))
+            {
+                Debug.LogError($"SteeringInput NaN. Setting 0");
+                SteeringInput = 0.0f;
+            }
+            else
+            {
+                SteeringInput = -(float)command.Lateral.Steering_tire_angle * Mathf.Rad2Deg;
+            }
+        }
+
         void OnDisable()
         {
             SimulatorROS2Node.RemoveSubscription<autoware_auto_vehicle_msgs.msg.TurnIndicatorsCommand>(turnIndicatorsCommandSubscriber);