diff --git a/Assets/Scripts/Bridge/Data/VehicleControlData.cs b/Assets/Scripts/Bridge/Data/VehicleControlData.cs index ca53f6d33..402a88e2c 100644 --- a/Assets/Scripts/Bridge/Data/VehicleControlData.cs +++ b/Assets/Scripts/Bridge/Data/VehicleControlData.cs @@ -30,6 +30,7 @@ public class VehicleControlData public float? SteerAngle; public bool ShiftGearUp; public bool ShiftGearDown; + public int? Indicator; // apollo public float? SteerRate; diff --git a/Assets/Scripts/Bridge/Ros/Messages/Autoware.cs b/Assets/Scripts/Bridge/Ros/Messages/Autoware.cs index ccb5b1832..5ff3dce42 100644 --- a/Assets/Scripts/Bridge/Ros/Messages/Autoware.cs +++ b/Assets/Scripts/Bridge/Ros/Messages/Autoware.cs @@ -5,7 +5,7 @@ * */ -#pragma warning disable 0649 +#pragma warning disable 0649 using System.Collections.Generic; @@ -56,7 +56,7 @@ public class VehicleCmd public steer_cmd _steer_cmd; public accel_cmd _accel_cmd; public brake_cmd _brake_cmd; - public lamp_cmd _lamp_cmd; + public lamp_cmd lamp_cmd; public uint gear; public uint mode; public TwistStamped twist_cmd; @@ -82,7 +82,7 @@ public class DetectedObject public Twist acceleration; public PointCloud2 pointcloud; - + // public PolygonStamped convex_hull; // public LaneArray candidate_trajectories; diff --git a/Assets/Scripts/Bridge/Ros/RosConversions.cs b/Assets/Scripts/Bridge/Ros/RosConversions.cs index c2f31a408..e5eef1c70 100644 --- a/Assets/Scripts/Bridge/Ros/RosConversions.cs +++ b/Assets/Scripts/Bridge/Ros/RosConversions.cs @@ -440,7 +440,7 @@ public static Odometry ConvertFrom(GpsOdometryData data) { x = 0.0, y = 0.0, - z = - data.AngularVelocity.y, + z = -data.AngularVelocity.y, } }, } @@ -534,6 +534,15 @@ public static VehicleControlData ConvertTo(Autoware.VehicleCmd data) shiftDown = true; } + + int indicator = 0; + if (data.lamp_cmd != null) + { + var lbrink = 0 < data.lamp_cmd.l ? 1 : 0; + var rbrink = 0 < data.lamp_cmd.r ? 1 : 0; + indicator = ((rbrink & 0x01) << 1) | (lbrink & 0x01); + } + return new VehicleControlData() { TimeStampSec = ConvertTime(data.header.stamp), @@ -544,6 +553,7 @@ public static VehicleControlData ConvertTo(Autoware.VehicleCmd data) SteerAngle = (float)data.ctrl_cmd.steering_angle, ShiftGearUp = shiftUp, ShiftGearDown = shiftDown, + Indicator = indicator }; } @@ -568,7 +578,7 @@ public static VehicleControlData ConvertTo(Lgsvl.VehicleControlDataRos data) wheelAngle = -MaxSteeringAngle; // ratio between -MaxSteeringAngle and MaxSteeringAngle - var k = (float)(wheelAngle + MaxSteeringAngle) / (MaxSteeringAngle*2); + var k = (float)(wheelAngle + MaxSteeringAngle) / (MaxSteeringAngle * 2); // target_wheel_angular_rate, target_gear are not supported on simulator side. return new VehicleControlData() @@ -619,11 +629,11 @@ public static Imu ConvertFrom(ImuData data) }, orientation = Convert(data.Orientation), - orientation_covariance = new double[9]{0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001}, + orientation_covariance = new double[9] { 0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001 }, angular_velocity = ConvertToVector(data.AngularVelocity), - angular_velocity_covariance = new double[9]{0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001}, + angular_velocity_covariance = new double[9] { 0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001 }, linear_acceleration = ConvertToVector(data.Acceleration), - linear_acceleration_covariance = new double[9]{0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001}, + linear_acceleration_covariance = new double[9] { 0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001 }, }; } diff --git a/Assets/Scripts/Controllers/VehicleController.cs b/Assets/Scripts/Controllers/VehicleController.cs index 70ad89b0e..885db1cce 100644 --- a/Assets/Scripts/Controllers/VehicleController.cs +++ b/Assets/Scripts/Controllers/VehicleController.cs @@ -84,13 +84,34 @@ private void UpdateInput() if (sticky) return; SteerInput = AccelInput = BrakeInput = 0f; - + // get all inputs foreach (var input in inputs) { SteerInput += input.SteerInput; AccelInput += input.AccelInput; BrakeInput += input.BrakeInput; + + var vcs = input as Simulator.Sensors.VehicleControlSensor; + if (vcs != null) + { + var hazard = vcs.Indicator == 3; + var rightTurn = vcs.Indicator == 2; + var leftTurn = vcs.Indicator == 1; + if (actions.HazardLights != hazard) + { + actions.HazardLights = hazard; + } + else if (actions.LeftTurnSignal != leftTurn) + { + actions.LeftTurnSignal = leftTurn; + } + else if (actions.RightTurnSignal != rightTurn) + { + actions.RightTurnSignal = rightTurn; + } + } + } // clamp if over @@ -102,7 +123,7 @@ private void UpdateInput() private void UpdateInputAPI() { if (!sticky) return; - + SteerInput = stickySteering; AccelInput = stickAcceleraton; } @@ -125,7 +146,7 @@ private void UpdateLights() { if (SteerInput > -turnSignalOffThreshold) actions.LeftTurnSignal = resetTurnIndicator = false; - + } else { diff --git a/Assets/Scripts/Sensors/VehicleControlSensor.cs b/Assets/Scripts/Sensors/VehicleControlSensor.cs index d39d735aa..86d230b02 100644 --- a/Assets/Scripts/Sensors/VehicleControlSensor.cs +++ b/Assets/Scripts/Sensors/VehicleControlSensor.cs @@ -30,6 +30,8 @@ public class VehicleControlSensor : SensorBase, IVehicleInputs public float AccelInput { get; private set; } = 0f; public float BrakeInput { get; private set; } = 0f; + public int Indicator { get; private set; } = 0; + float ADAccelInput = 0f; float ADSteerInput = 0f; @@ -107,6 +109,11 @@ public override void OnBridgeSetup(IBridge bridge) ADAccelInput = ActualLinVel < data.Velocity.GetValueOrDefault() ? linMag : -linMag; ADSteerInput = -Mathf.Clamp(data.SteerAngularVelocity.GetValueOrDefault() * 0.5f, -1f, 1f); } + + if (data.Indicator.HasValue) + { + Indicator = data.Indicator.Value; + } } else if (data.SteerRate.HasValue) // apollo { @@ -130,7 +137,7 @@ public override void OnBridgeSetup(IBridge bridge) var sgn = Mathf.Sign(steeringTarget - steeringAngle); var steeringRate = data.SteerRate.GetValueOrDefault() * sgn; steeringAngle += steeringRate * dt; - + if (sgn != steeringTarget - steeringAngle) // to prevent oversteering steeringAngle = steeringTarget;