Merge pull request #308 from tier4/feature/vehicle-override-input

Override input for vehicle

Assets/AWSIM/Scripts/Sensors/VehicleReportRos2Publisher.cs

@@ -54,6 +54,7 @@ public class VehicleReportRos2Publisher : MonoBehaviour
         [SerializeField] QoSSettings qosSettings;
 
         [SerializeField] Vehicle vehicle;
+        [SerializeField] VehicleOverrideInputManager VehicleOverrideInputManager;
 
         // msgs.
         autoware_auto_vehicle_msgs.msg.ControlModeReport controlModeReportMsg;
@@ -118,7 +119,8 @@ void FixedUpdate()
             timer = 0;
 
             // ControlModeReport
-            controlModeReportMsg.Mode = autoware_auto_vehicle_msgs.msg.ControlModeReport.AUTONOMOUS;
+            var controlMode = VehicleROS2Utility.UnityToRosControlMode(VehicleOverrideInputManager.ControlMode);
+            controlModeReportMsg.Mode = controlMode;
 
             // GearReport
             gearReportMsg.Report = VehicleROS2Utility.UnityToRosShift(vehicle.AutomaticShiftInput);

Assets/AWSIM/Scripts/UI/ControlModeUI.cs

@@ -0,0 +1,18 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.UI;
+
+namespace AWSIM
+{
+    public class ControlModeUI : MonoBehaviour
+    {
+        [SerializeField] VehicleOverrideInputManager vehicleOverrideInputManager;
+        [SerializeField] Text text;
+
+        void Update()
+        {
+            text.text = vehicleOverrideInputManager.ControlMode.ToString();
+        }
+    }
+}

Assets/AWSIM/Scripts/Vehicles/EngageSubscriber.cs

@@ -0,0 +1,40 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using ROS2;
+
+namespace AWSIM
+{
+    /// <summary>
+    /// Subscribe to the "/vehicle/engage" topic and change the vehicle's control mode to AUTONOMOUS.
+    /// </summary>
+    public class EngageSubscriber : MonoBehaviour
+    {
+        [SerializeField] string engageTopic = "/vehicle/engage";
+        [SerializeField] QoSSettings qosSettings = new QoSSettings();
+        ISubscription<autoware_auto_vehicle_msgs.msg.Engage> engageSubscriber;
+
+        [SerializeField] VehicleOverrideInputManager vehicleOverrideInputManager;
+        void Reset()
+        {
+            qosSettings.ReliabilityPolicy = ReliabilityPolicy.QOS_POLICY_RELIABILITY_RELIABLE;
+            qosSettings.DurabilityPolicy = DurabilityPolicy.QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+            qosSettings.HistoryPolicy = HistoryPolicy.QOS_POLICY_HISTORY_KEEP_LAST;
+            qosSettings.Depth = 1;
+        }
+
+        void OnEnable()
+        {
+            var qos = qosSettings.GetQoSProfile();
+
+            engageSubscriber = SimulatorROS2Node.CreateSubscription<autoware_auto_vehicle_msgs.msg.Engage>(
+                engageTopic, msg =>
+                {
+                    if (msg.Engage_)
+                    {
+                        vehicleOverrideInputManager.ChangeControlModeToAUTONOMOUS();
+                    }
+                }, qos);
+        }
+    }
+}

Assets/AWSIM/Scripts/Vehicles/Vehicle.cs

@@ -170,12 +170,12 @@ public float SidewaySlipMultipler
         // Set value to clamp SteerAngleInput (degree).
         // -MaxSteerAngleInput <= SteerAngleInput <= MaxSteerAngleInput.
         [Range(0.01f, 80)]
-        [SerializeField] float MaxSteerAngleInput = 35f;
+        public float MaxSteerAngleInput = 35f;
 
         // Set value to clamp AccelerationInput (m/s^2).
         // -MaxAccelerationInput <= AccelerationInput <= MaxAccelerationInput.
         [Range(0.01f, 50)]
-        [SerializeField] float MaxAccelerationInput = 10;
+        public float MaxAccelerationInput = 10;
 
         [Header("Inputs")]
 

Assets/AWSIM/Scripts/Vehicles/VehicleControlMode.cs

@@ -0,0 +1,22 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace AWSIM
+{
+    /// <summary>
+    /// C# enum for Autoware's ControlMode. (No dependency on ROS2)
+    /// VehicleROS2Utility.UnityToRosControlMode() can be used to convert C# enum to ros msg.
+    /// <see href="https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/ControlModeReport.msg"/>
+    /// </summary>
+    public enum VehicleControlMode
+    {
+        // NO_COMMAND = 0,
+        AUTONOMOUS = 1,
+        // AUTONOMOUS_STEER_ONLY = 2,
+        // AUTONOMOUS_VELOCITY_ONLY = 3,
+        MANUAL = 4,
+        // DISENGAGED = 5,
+        // NOT_READY = 6
+    }
+}

Assets/AWSIM/Scripts/Vehicles/VehicleInputBase.cs

@@ -0,0 +1,57 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace AWSIM
+{
+    /// <summary>
+    /// Input base class to the vehicle.
+    /// By creating arbitrary input classes to the vehicle (keyboard, steering controller, etc.) 
+    /// that inherit from this class, you can reproduce actual vehicle input overrides.
+    /// Actual vehicle input is performed by VehicleOverrideInputManager. 
+    /// </summary>
+    [RequireComponent(typeof(VehicleOverrideInputManager))]
+    public class VehicleInputBase : MonoBehaviour
+    {
+        /// <summary>
+        /// Acceleration(m/s^2) to be applied to Vehicle acquired from Input.
+        /// </summary>
+        public float AccelerationInput { get; protected set; } = 0;
+
+        /// <summary>
+        /// Steering to be applied to Vehicle acquired from Input.
+        /// </summary>
+        public float SteeringInput { get; protected set; } = 0;
+
+        /// <summary>
+        /// Shift to be applied to Vehicle acquired from Input.
+        /// </summary>
+        public Vehicle.Shift ShiftInput { get; protected set; } = Vehicle.Shift.PARKING;
+
+        /// <summary>
+        /// TurnSignal to be applied to Vehicle acquired from Input.
+        /// </summary>
+        public Vehicle.TurnSignal TurnSignalInput { get; protected set; } = Vehicle.TurnSignal.NONE;
+
+        /// <summary>
+        /// Is there an input override to the vehicle?
+        /// </summary>
+        public bool Overridden { get; protected set; } = false;
+
+        /// <summary>
+        /// New control mode when input override occurs.
+        /// Used by VehicleOverrideInputManager class when Overridden property is true.
+        /// </summary>
+        public VehicleControlMode NewControlMode { get; protected set; } = VehicleControlMode.AUTONOMOUS;
+
+        /// <summary>
+        /// Instead of unity callback Update(), this method is overridden and used to get inputs.
+        /// This method is called by the VehicleOverrideInputManager class.
+        /// </summary>
+        /// <param name="currentControlMode"></param>
+        public virtual void OnUpdate(VehicleControlMode currentControlMode)
+        {
+            // Override this method to get inputs!
+        }
+    }
+}

Assets/AWSIM/Scripts/Vehicles/VehicleKeyboardInput.cs

@@ -5,66 +5,77 @@
 namespace AWSIM
 {
     /// <summary>
-    /// This is a sample class for controlling a vehicle with a keyboard.
+    /// Controlling vehicle input via keyboard.
     /// </summary>
-
-    // ----- key binds -----
-    // up arrow : Accelerate
-    // down arrow : Deceleration
-    // left/right arrow : Steering
-    // D : Drive gear
-    // P : Parking gear
-    // R : Reverse gear
-    // N : Neutral gear
-    // 1 : Left turn signal
-    // 2 : Right turn signal
-    // 3 : Hazard
-    // 4 : Turn signal off
-    [RequireComponent(typeof(Vehicle))]
-    public class VehicleKeyboardInput : MonoBehaviour
+    /// ----- key binds -----
+    /// up arrow : Accelerate
+    /// down arrow : Deceleration
+    /// left/right arrow : Steering
+    /// D : Drive gear
+    /// P : Parking gear
+    /// R : Reverse gear
+    /// N : Neutral gear
+    /// 1 : Left turn signal
+    /// 2 : Right turn signal
+    /// 3 : Hazard
+    /// 4 : Turn signal off
+    public class VehicleKeyboardInput : VehicleInputBase
     {
+        public float MaxAcceleration = 1.5f;
+        float MaxSteerAngle = 0;
         [SerializeField] Vehicle vehicle;
 
-        [SerializeField] float maxAcceleration = 1.5f;
-        [SerializeField] float maxSteerAngle = 35;
-
-        void Reset()
+        void OnEnable()
         {
-            if (vehicle == null)
-                vehicle = GetComponent<Vehicle>();
+            MaxSteerAngle = vehicle.MaxSteerAngleInput;
         }
 
-        void Update()
+        public override void OnUpdate(VehicleControlMode currentControlMode)
         {
-            // get arrow inputs
+            Overridden = false;
+
+            // Get steering and acceleration input.
             var horizontal = Input.GetAxis("Horizontal");
             var vertical = Input.GetAxis("Vertical");
+            AccelerationInput = MaxAcceleration * vertical;
+            SteeringInput = MaxSteerAngle * horizontal;
 
-            // set acceleration
-            vehicle.AccelerationInput = maxAcceleration * vertical;
-
-            // set steer
-            vehicle.SteerAngleInput = maxSteerAngle * horizontal;
-
-            // set gear
+            // Get gear input.
             if (Input.GetKey(KeyCode.D))
-                vehicle.AutomaticShiftInput = Vehicle.Shift.DRIVE;
+                ShiftInput = Vehicle.Shift.DRIVE;
             else if (Input.GetKey(KeyCode.P))
-                vehicle.AutomaticShiftInput = Vehicle.Shift.PARKING;
+                ShiftInput = Vehicle.Shift.PARKING;
             else if (Input.GetKey(KeyCode.R))
-                vehicle.AutomaticShiftInput = Vehicle.Shift.REVERSE;
+                ShiftInput = Vehicle.Shift.REVERSE;
             else if (Input.GetKey(KeyCode.N))
-                vehicle.AutomaticShiftInput = Vehicle.Shift.NEUTRAL;
+                ShiftInput = Vehicle.Shift.NEUTRAL;
+            else
+                ShiftInput = vehicle.AutomaticShift;    // No new input.
 
-            // set turn signal
+            // Get turn signal input.
             if (Input.GetKey(KeyCode.Alpha1))
-                vehicle.SignalInput = Vehicle.TurnSignal.LEFT;
+                TurnSignalInput = Vehicle.TurnSignal.LEFT;
             else if (Input.GetKey(KeyCode.Alpha2))
-                vehicle.SignalInput = Vehicle.TurnSignal.RIGHT;
+                TurnSignalInput = Vehicle.TurnSignal.RIGHT;
             else if (Input.GetKey(KeyCode.Alpha3))
-                vehicle.SignalInput = Vehicle.TurnSignal.HAZARD;
+                TurnSignalInput = Vehicle.TurnSignal.HAZARD;
             else if (Input.GetKey(KeyCode.Alpha4))
-                vehicle.SignalInput = Vehicle.TurnSignal.NONE;
+                TurnSignalInput = Vehicle.TurnSignal.NONE;
+            else
+                TurnSignalInput = vehicle.Signal;       // No new input.
+
+            // Input override considerations.
+            if (currentControlMode == VehicleControlMode.AUTONOMOUS)
+            {
+                // Override the vehicle's control mode when a steering input or acceleration input is given.
+                if (Mathf.Abs(horizontal) > 0 || Mathf.Abs(vertical) > 0)
+                {
+                    Overridden = true;
+                    NewControlMode = VehicleControlMode.MANUAL;
+                }
+
+                // TODO: Implement switches to other overrides.
+            }
         }
     }
 }