Skip to content
Permalink

Comparing changes

Choose two branches to see what’s changed or to start a new pull request. If you need to, you can also or learn more about diff comparisons.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also . Learn more about diff comparisons here.
base repository: autowarefoundation/AWSIM-Labs
Failed to load repositories. Confirm that selected base ref is valid, then try again.
Loading
base: v1.3.1
Choose a base ref
...
head repository: autowarefoundation/AWSIM-Labs
Failed to load repositories. Confirm that selected head ref is valid, then try again.
Loading
compare: v1.4.0
Choose a head ref
  • 1 commit
  • 40 files changed
  • 1 contributor

Commits on Sep 13, 2024

  1. feat: VPP CE Integration (#116)

    Signed-off-by: Alptuğ Cırıt <alptug@leodrive.ai>
    mozhoku authored Sep 13, 2024

    Verified

    This commit was created on GitHub.com and signed with GitHub’s verified signature.
    Copy the full SHA
    ac4ff08 View commit details
Showing with 3,200 additions and 809 deletions.
  1. +1,009 −736 Assets/AWSIM/Prefabs/Vehicles/Lexus RX450h 2015 Sample Sensor.prefab
  2. +368 −10 Assets/AWSIM/Scenes/Composition/AWSIMSimulation.unity
  3. +371 −13 Assets/AWSIM/Scenes/Main/AutowareSimulation.unity
  4. +10 −6 Assets/AWSIM/Scripts/UI/Toggle/UIKeyboardControlToggle.cs
  5. +0 −2 Assets/AWSIM/Scripts/UI/UICard.cs
  6. +1 −1 ...Materials/New Material.mat.meta~refs/remotes/origin.meta → Scripts/Vehicles/VPP Integration.meta}
  7. +343 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/AutowareVPPAdapter.cs
  8. +11 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/AutowareVPPAdapter.cs.meta
  9. +8 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/Enums.meta
  10. +13 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/Enums/VPPControlMode.cs
  11. +11 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/Enums/VPPControlMode.cs.meta
  12. +10 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/Enums/VPPSignal.cs
  13. +11 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/Enums/VPPSignal.cs.meta
  14. +8 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/IVehicleControlModes.meta
  15. +74 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/IVehicleControlModes/ControlMode.cs
  16. +11 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/IVehicleControlModes/ControlMode.cs.meta
  17. +8 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/IVehicleControlModes/IVehicleControlMode.cs
  18. +11 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/IVehicleControlModes/IVehicleControlMode.cs.meta
  19. +173 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/Ros2ToVPPInput.cs
  20. +11 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/Ros2ToVPPInput.cs.meta
  21. +112 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/Ros2ToVPPUtilities.cs
  22. +11 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/Ros2ToVPPUtilities.cs.meta
  23. +186 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/VPPToRos2Publisher.cs
  24. +11 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/VPPToRos2Publisher.cs.meta
  25. +151 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/VPPVehicleSignalHandler.cs
  26. +11 −0 Assets/AWSIM/Scripts/Vehicles/VPP Integration/VPPVehicleSignalHandler.cs.meta
  27. +33 −26 Assets/AWSIM/Scripts/Vehicles/VehicleVisualEffect.cs
  28. +8 −0 Assets/Vehicle Physics Pro.meta
  29. +7 −0 Assets/Vehicle Physics Pro/.gitignore
  30. +1 −0 Assets/Vehicle Physics Pro/Editor/csc.rsp
  31. +7 −0 Assets/Vehicle Physics Pro/Editor/csc.rsp.meta
  32. +6 −6 ProjectSettings/DynamicsManager.asset
  33. +1 −1 ProjectSettings/ProjectSettings.asset
  34. +4 −4 ProjectSettings/QualitySettings.asset
  35. +1 −1 docs/Components/Vehicle/EgoVehicle/index.md
  36. +146 −0 docs/Components/Vehicle/VPPIntegration/index.md
  37. +28 −0 docs/DeveloperGuide/EditorSetup/VPPCommunityEdition/index.md
  38. +8 −3 docs/GettingStarted/QuickStartDemo/index.md
  39. +4 −0 docs/GettingStarted/SetupUnityProject/index.md
  40. +2 −0 mkdocs.yml
1,745 changes: 1,009 additions & 736 deletions Assets/AWSIM/Prefabs/Vehicles/Lexus RX450h 2015 Sample Sensor.prefab

Large diffs are not rendered by default.

378 changes: 368 additions & 10 deletions Assets/AWSIM/Scenes/Composition/AWSIMSimulation.unity

Large diffs are not rendered by default.

384 changes: 371 additions & 13 deletions Assets/AWSIM/Scenes/Main/AutowareSimulation.unity

Large diffs are not rendered by default.

16 changes: 10 additions & 6 deletions Assets/AWSIM/Scripts/UI/Toggle/UIKeyboardControlToggle.cs
Original file line number Diff line number Diff line change
@@ -1,19 +1,21 @@
using AWSIM.Scripts.Vehicles.VPP_Integration;
using AWSIM.Scripts.Vehicles.VPP_Integration.Enums;
using UnityEngine;

namespace AWSIM.Scripts.UI.Toggle
{
public class UIKeyboardControlToggle : MonoBehaviour
{
private GameObject _egoVehicle;
private VehicleKeyboardInput _vehicleKeyboardInput;
private AutowareVPPAdapter _adapter;

private void Start()
{
_egoVehicle = GameObject.FindGameObjectWithTag("Ego");
_vehicleKeyboardInput = _egoVehicle.GetComponent<VehicleKeyboardInput>();
_egoVehicle = GameObject.FindWithTag("Ego");
_adapter = _egoVehicle.GetComponent<AutowareVPPAdapter>();

// Set the toggle to the current state of the keyboard control
GetComponent<UnityEngine.UI.Toggle>().isOn = _vehicleKeyboardInput.enabled;
// Set the toggle to the current state of the keyboard control mode
GetComponent<UnityEngine.UI.Toggle>().isOn = _adapter.ControlModeInput != VPPControlMode.Autonomous;
}

public void Activate()
@@ -25,7 +27,9 @@ public void Activate()
// Toggle the keyboard control
public void OnClick(bool isOn)
{
_vehicleKeyboardInput.enabled = isOn;
_adapter.ControlModeInput = _adapter.ControlModeInput == VPPControlMode.Autonomous
? VPPControlMode.Manual
: VPPControlMode.Autonomous;
}
}
}
2 changes: 0 additions & 2 deletions Assets/AWSIM/Scripts/UI/UICard.cs
Original file line number Diff line number Diff line change
@@ -8,8 +8,6 @@ public class UICard : MonoBehaviour
{
[SerializeField] private Button _cardTopBarButton;
[SerializeField] private RectTransform _barChevronRect;
[SerializeField] private float _chevronExpandedDegrees = 90f;
[SerializeField] private float _chevronCollapsedDegrees = 0f;

[SerializeField] private bool _isCardOpen;
[SerializeField] public float ElementHeight = 30f;
343 changes: 343 additions & 0 deletions Assets/AWSIM/Scripts/Vehicles/VPP Integration/AutowareVPPAdapter.cs
Original file line number Diff line number Diff line change
@@ -0,0 +1,343 @@
using System;
using System.Collections.Generic;
using AWSIM.Scripts.Vehicles.VPP_Integration.Enums;
using AWSIM.Scripts.Vehicles.VPP_Integration.IVehicleControlModes;
using UnityEngine;
using VehiclePhysics;

namespace AWSIM.Scripts.Vehicles.VPP_Integration
{
public class AutowareVPPAdapter : MonoBehaviour
{
/// <summary>
/// This script applies the VPP vehicle inputs.
/// Vehicle inputs are updated from the `Ros2ToVPPInput.cs`.
/// Results from this script are sent to the `VPPtoRos2Publisher.cs`.
/// </summary>

// Initial Position inputs from Rviz
[NonSerialized] public bool WillUpdatePositionInput;

[NonSerialized] public Vector3 PositionInput;
[NonSerialized] public Quaternion RotationInput;

/// Control inputs from Autoware
[NonSerialized] public float SteerAngleInput;

private float _steerAngleInput => SteerAngleInput;

public bool IsDefinedSteeringTireRotationRateInput { get; set; }
public float SteeringTireRotationRateInput { get; set; }

// Gear input from Autoware
[NonSerialized] public Gearbox.AutomaticGear AutomaticShiftInput;
private Gearbox.AutomaticGear _automaticShiftInput => AutomaticShiftInput;

// Signal input from Autoware
[NonSerialized] public VPPSignal VehicleSignalInput;
private VPPSignal _vehicleSignalInput => VehicleSignalInput;

// Emergency input from Autoware
public bool IsEmergencyInput { get; set; }

// Control mode input from Autoware
public VPPControlMode ControlModeInput { get; set; }

// Actuation commands from Autoware
public double SteerInput { get; set; }
[NonSerialized] public double BrakeInput;
private double _brakeInput => BrakeInput;
[NonSerialized] public double ThrottleInput;
private double _throttleInput => ThrottleInput;

// Outputs from Unity/VPP
public VPPControlMode VpControlModeReport { get; private set; }
public int VpGearReport { get; private set; }
public Vector3 VpVelocityReport { get; private set; }
public Vector3 VpAngularVelocityReport { get; private set; }
public float VpSteeringReport { get; private set; }
public VPPSignal VpTurnIndicatorReport { get; private set; }
public VPPSignal VpHazardLightsReport { get; private set; }
public double VpThrottleStatusReport { get; private set; }
public double VpBrakeStatusReport { get; private set; }
public double VpSteerStatusReport { get; private set; }

// VPP components
private VPVehicleController _vehicleController;
private VPStandardInput _standardInput;
private VPCameraController _cameraController;

[SerializeField] private VPWheelCollider[] _frontWheels;

private Rigidbody _rigidbody;

[Tooltip("Whether set wheel angle directly from Autoware or simulate with additive steering wheel input")]
[SerializeField]
private bool _simulateSteering;

[Tooltip("Change applied to steering wheel per fixed update in degrees")]
[Range(0f, 100f)]
[SerializeField]
private float _steerWheelInput = 5f;

private int _vppSteerFromLastFrame;

[Tooltip("Brake pedal percent on emergency brake. This value is mapped to [0, 10000] to match VPP input")]
[Range(0f, 100f)]
[SerializeField]
private float _emergencyBrakePercent = 100f;

public float CurrentSpeed { get; private set; }
public float CurrentJerk { get; private set; }
// private float _previousAcceleration;

// Control mode variables
private Dictionary<VPPControlMode, IVehicleControlMode> _controlModes;
private IVehicleControlMode _currentMode;

// RViz2 Update position variables
[Header("RViz2 Update Position")]
[SerializeField]
private float _updatePositionOffsetY = 1.33f;

[SerializeField] private float _updatePositionRayOriginY = 1000f;

[Header("Calibration Mode")]
[Tooltip(
"Enable pedal calibration mode. Use Numpad(+,-,0) keys to set constant throttle/brake values for ease of calibration")]
[SerializeField]
private bool _doPedalCalibration;

private int _brakeAmount;
private int _throttleAmount;

// Constants used for conversion between VPP and Autoware
private const float VppToAutowareMultiplier = 0.0001f;
private const int AutowareToVppMultiplier = 10000;

private void Awake()
{
//TODO: Implement the control mode switch from simulator (mozzz)
// Initialize the control mode as Autonomous
ControlModeInput = VPPControlMode.Autonomous;
}

private void Start()
{
// get components
_vehicleController = GetComponent<VPVehicleController>();
_standardInput = GetComponent<VPStandardInput>();
_rigidbody = GetComponent<Rigidbody>();

// set camera target to this vehicle
_cameraController = FindObjectOfType<VPCameraController>();
_cameraController.target = transform;

InitializeControlModes();

// Set initial vehicle gear to Park to prevent movement
_vehicleController.data.bus[Channel.Input][InputData.AutomaticGear] = (int)Gearbox.AutomaticGear.P;

// reset vpp stuff for multi-scene loading (no idea why, but it works)
_vehicleController.enabled = false;
_vehicleController.enabled = true;
}

// private void Update()
// {
// // TODO: Implement the control mode switch from simulator (mozzz)
// UserSwitchControlMode();
// }

private void FixedUpdate()
{
// Update the ego position depending on RViz Input.
if (WillUpdatePositionInput)
{
UpdateEgoPosition();
WillUpdatePositionInput = false;
}

if (_doPedalCalibration)
{
PedalCalibrationMode();
}
else
{
// If keyboard enabled stop Autoware control.
// if (_standardInput.enabled) return;

// Control the vehicle based on the control mode
ControlVehicle(ControlModeInput);
}

// Update the publisher values for VPPToRos2Publisher.cs
ReportVehicleState();
}

private void InitializeControlModes()
{
_controlModes = new Dictionary<VPPControlMode, IVehicleControlMode>
{
{ VPPControlMode.NoCommand, new ControlMode.NoCommand() },
{ VPPControlMode.Autonomous, new ControlMode.Autonomous() },
{ VPPControlMode.AutonomousSteerOnly, new ControlMode.AutonomousSteerOnly() },
{ VPPControlMode.AutonomousVelocityOnly, new ControlMode.AutonomousVelocityOnly() },
{ VPPControlMode.Manual, new ControlMode.Manual() },
{ VPPControlMode.Disengaged, new ControlMode.Disengaged() },
{ VPPControlMode.NotReady, new ControlMode.NotReady() }
};
}

private void ControlVehicle(VPPControlMode controlMode)
{
if (_controlModes.TryGetValue(controlMode, out _currentMode))
{
_currentMode.ExecuteControlMode(this);
}
else
{
Debug.LogWarning("Control mode is not recognized.");
}
}

public void HandleHazardLights()
{
// TODO: this is implemented in Ros2ToVPPInput.cs, move it here (mozzz)
}

public void HandleTurnSignal()
{
// TODO: this is implemented in Ros2ToVPPInput.cs, move it here (mozzz)
}

public void HandleSteer()
{
if (_simulateSteering)
{
SimulateSteeringWheelInput();
}
else
{
SetWheelSteerAngleDirectly(_steerAngleInput);
}
}

private void SetWheelSteerAngleDirectly(float angle)
{
foreach (var wheel in _frontWheels)
{
wheel.steerAngle = angle;
}
}

// temp method to simulate steering wheel input with delay, also inaccurate (mozzz)
private void SimulateSteeringWheelInput()
{
if (SteerAngleInput == 0) return;

int steerAdjustment = SteerAngleInput > _vehicleController.wheelState[0].steerAngle
? (int)_steerWheelInput
: -(int)_steerWheelInput;

_vehicleController.data.bus[Channel.Input][InputData.Steer] = _vppSteerFromLastFrame + steerAdjustment;
_vppSteerFromLastFrame = _vehicleController.data.bus[Channel.Input][InputData.Steer];
}

public void HandleGear()
{
_vehicleController.data.bus[Channel.Input][InputData.AutomaticGear] = (int)_automaticShiftInput;
}

public void HandleAcceleration()
{
// Store current values
CurrentSpeed = _vehicleController.speed;

SetThrottle((int)(_throttleInput * AutowareToVppMultiplier));
SetBrake((int)(_brakeInput * AutowareToVppMultiplier));
}

/// <summary>
/// Range:[0-10000]
/// </summary>
private void SetThrottle(int amount)
{
_vehicleController.data.bus[Channel.Input][InputData.Throttle] = amount;
}

/// <summary>
/// Range:[0-10000]
/// </summary>
private void SetBrake(int amount)
{
_vehicleController.data.bus[Channel.Input][InputData.Brake] = amount;
}

// TODO: report jerk state (mozzz)
private void ReportVehicleState()
{
VpControlModeReport = ControlModeInput;
VpHazardLightsReport = _vehicleSignalInput;
VpTurnIndicatorReport = _vehicleSignalInput;
VpSteeringReport = _frontWheels[0].steerAngle;
VpGearReport = _vehicleController.data.bus[Channel.Vehicle][VehicleData.GearboxMode];
VpVelocityReport =
transform.InverseTransformDirection(_rigidbody.velocity.normalized * _vehicleController.speed);
VpAngularVelocityReport = transform.InverseTransformDirection(_rigidbody.angularVelocity);
VpThrottleStatusReport =
_vehicleController.data.bus[Channel.Input][InputData.Throttle] * VppToAutowareMultiplier;
VpBrakeStatusReport = _vehicleController.data.bus[Channel.Input][InputData.Brake] * VppToAutowareMultiplier;
VpSteerStatusReport = _vehicleController.data.bus[Channel.Input][InputData.Steer] * VppToAutowareMultiplier;
}

private void UpdateEgoPosition()
{
// Method to update the position based on PositionInput
Vector3 rayOrigin = new Vector3(PositionInput.x, _updatePositionRayOriginY, PositionInput.z);
Vector3 rayDirection = Vector3.down;

if (Physics.Raycast(rayOrigin, rayDirection, out RaycastHit hit, Mathf.Infinity))
{
PositionInput = new Vector3(PositionInput.x, hit.point.y + _updatePositionOffsetY, PositionInput.z);
transform.SetPositionAndRotation(PositionInput, RotationInput);
}
else
{
Debug.LogWarning(
"No mesh or collider detected on target location. Please ensure that the target location is on a mesh or collider.");
}
}

// TODO: Method to switch control mode based on user input (mozzz)
private void UserSwitchControlMode()
{
}

// TODO: Add UI and documentation for this method (mozzz)
/// <summary>
/// Used to enable Numpad(+,-,0) keys to set constant throttle/brake values for ease of calibration. %10 increments
/// </summary>
private void PedalCalibrationMode()
{
_vehicleController.data.bus[Channel.Input][InputData.Throttle] = _throttleAmount;
_vehicleController.data.bus[Channel.Input][InputData.Brake] = _brakeAmount;

if (Input.GetKeyDown(KeyCode.KeypadPlus))
{
_throttleAmount += 1000;
}

if (Input.GetKeyDown(KeyCode.KeypadMinus))
{
_brakeAmount += 1000;
}

if (Input.GetKeyDown(KeyCode.Keypad0))
{
_throttleAmount = 0;
_brakeAmount = 0;
}
}
}
}
8 changes: 8 additions & 0 deletions Assets/AWSIM/Scripts/Vehicles/VPP Integration/Enums.meta
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
namespace AWSIM.Scripts.Vehicles.VPP_Integration.Enums
{
public enum VPPControlMode
{
NoCommand,
Autonomous,
AutonomousSteerOnly,
AutonomousVelocityOnly,
Manual,
Disengaged,
NotReady
}
}
10 changes: 10 additions & 0 deletions Assets/AWSIM/Scripts/Vehicles/VPP Integration/Enums/VPPSignal.cs
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
namespace AWSIM.Scripts.Vehicles.VPP_Integration.Enums
{
public enum VPPSignal
{
None,
Left,
Right,
Hazard
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,74 @@
using UnityEngine;

namespace AWSIM.Scripts.Vehicles.VPP_Integration.IVehicleControlModes
{
// Vehicle control mode classes
public abstract class ControlMode
{
public class NoCommand : IVehicleControlMode
{
public void ExecuteControlMode(AutowareVPPAdapter adapter)
{
// Debug.Log("Control mode: No command");
}
}

public class Autonomous : IVehicleControlMode
{
public void ExecuteControlMode(AutowareVPPAdapter adapter)
{
adapter.HandleHazardLights();
adapter.HandleTurnSignal();
adapter.HandleAcceleration();
adapter.HandleGear();
adapter.HandleSteer();
}
}

public class AutonomousSteerOnly : IVehicleControlMode
{
public void ExecuteControlMode(AutowareVPPAdapter adapter)
{
adapter.HandleHazardLights();
adapter.HandleTurnSignal();
adapter.HandleGear();
adapter.HandleSteer();
}
}

public class AutonomousVelocityOnly : IVehicleControlMode
{
public void ExecuteControlMode(AutowareVPPAdapter adapter)
{
adapter.HandleHazardLights();
adapter.HandleTurnSignal();
adapter.HandleAcceleration();
adapter.HandleGear();
}
}

public class Manual : IVehicleControlMode
{
public void ExecuteControlMode(AutowareVPPAdapter adapter)
{
// Debug.Log("Control mode: Manual");
}
}

public class Disengaged : IVehicleControlMode
{
public void ExecuteControlMode(AutowareVPPAdapter adapter)
{
// Debug.Log("Control mode: Disengaged");
}
}

public class NotReady : IVehicleControlMode
{
public void ExecuteControlMode(AutowareVPPAdapter adapter)
{
// Debug.Log("Control mode: Not ready");
}
}
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
namespace AWSIM.Scripts.Vehicles.VPP_Integration.IVehicleControlModes
{
// Vehicle control mode interface
public interface IVehicleControlMode
{
void ExecuteControlMode(AutowareVPPAdapter adapter);
}
}
173 changes: 173 additions & 0 deletions Assets/AWSIM/Scripts/Vehicles/VPP Integration/Ros2ToVPPInput.cs
Original file line number Diff line number Diff line change
@@ -0,0 +1,173 @@
using AWSIM.Scripts.Vehicles.VPP_Integration.Enums;
using ROS2;
using UnityEngine;

namespace AWSIM.Scripts.Vehicles.VPP_Integration
{
[RequireComponent(typeof(AutowareVPPAdapter))]
public class Ros2ToVPPInput : MonoBehaviour
{
[Header("AutowareVPPAdapter")]
[SerializeField]
private AutowareVPPAdapter _adapter;

[Header("Ros2 fields")]
[SerializeField]
private string controlModeTopic = "/vehicle/status/control_mode";

[SerializeField] private string turnIndicatorsCommandTopic = "/control/command/turn_indicators_cmd";
[SerializeField] private string hazardLightsCommandTopic = "/control/command/hazard_lights_cmd";
[SerializeField] private string controlCommandTopic = "/control/command/control_cmd";
[SerializeField] private string gearCommandTopic = "/control/command/gear_cmd";
[SerializeField] private string vehicleEmergencyStampedTopic = "/control/command/emergency_cmd";
[SerializeField] private string positionTopic = "/initialpose";
[SerializeField] private string actuationCommandTopic = "/control/command/actuation_cmd";

[SerializeField] private QoSSettings qosSettings = new();
[SerializeField] private QoSSettings positionQosSettings;
[SerializeField] private QoSSettings _actuationQosSettings;

// subscribers
private ISubscription<autoware_vehicle_msgs.msg.ControlModeReport> _controlModeSubscriber;
private ISubscription<autoware_vehicle_msgs.msg.TurnIndicatorsCommand> _turnIndicatorsCommandSubscriber;
private ISubscription<autoware_vehicle_msgs.msg.HazardLightsCommand> _hazardLightsCommandSubscriber;
private ISubscription<autoware_control_msgs.msg.Control> _controlCommandSubscriber;
private ISubscription<autoware_vehicle_msgs.msg.GearCommand> _gearCommandSubscriber;
private ISubscription<tier4_vehicle_msgs.msg.VehicleEmergencyStamped> _vehicleEmergencyStampedSubscriber;
private ISubscription<geometry_msgs.msg.PoseWithCovarianceStamped> _positionSubscriber;
private ISubscription<tier4_vehicle_msgs.msg.ActuationCommandStamped> _actuationCommandSubscriber;

private VPPSignal _turnIndicatorsCommand = VPPSignal.None;
private VPPSignal _hazardLightsCommand = VPPSignal.None;
private VPPSignal _input = VPPSignal.None;

private void Reset()
{
if (_adapter == null)
_adapter = GetComponent<AutowareVPPAdapter>();

// initialize default QoS params.
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;
}

// TODO: handle this input in VPPVehicleSignalHandler.cs (mozzz)
/// <summary>
/// Processes the TurnSignal to be applied to the vehicle from the latest turnIndicatorsSignal and hazardLightsSignal values.
/// Priority : HAZARD > LEFT/RIGHT > NONE
/// </summary>
private void UpdateVehicleTurnSignal()
{
// HAZARD > LEFT, RIGHT > NONE
if (_hazardLightsCommand == VPPSignal.Hazard)
_input = _hazardLightsCommand;
else if (_turnIndicatorsCommand is VPPSignal.Left or VPPSignal.Right)
_input = _turnIndicatorsCommand;
else
_input = VPPSignal.None;

// input
if (!Equals(_adapter.VehicleSignalInput, _input))
_adapter.VehicleSignalInput = _input;
}

private void Start()
{
var qos = qosSettings.GetQoSProfile();
var positionQoS = positionQosSettings.GetQoSProfile();
var actuationQoSSettings = _actuationQosSettings.GetQoSProfile();

_controlModeSubscriber =
SimulatorROS2Node.CreateSubscription<autoware_vehicle_msgs.msg.ControlModeReport>(
controlModeTopic,
msg => { _adapter.ControlModeInput = Ros2ToVPPUtilities.Ros2ToVPPControlMode(msg); }, qos);

_turnIndicatorsCommandSubscriber =
SimulatorROS2Node.CreateSubscription<autoware_vehicle_msgs.msg.TurnIndicatorsCommand>(
turnIndicatorsCommandTopic,
msg =>
{
_turnIndicatorsCommand = Ros2ToVPPUtilities.Ros2ToVPPTurnSignal(msg);
UpdateVehicleTurnSignal();
}, qos);

_hazardLightsCommandSubscriber =
SimulatorROS2Node.CreateSubscription<autoware_vehicle_msgs.msg.HazardLightsCommand>(
hazardLightsCommandTopic,
msg =>
{
_hazardLightsCommand = Ros2ToVPPUtilities.Ros2ToVPPHazard(msg);
UpdateVehicleTurnSignal();
}, qos);

_controlCommandSubscriber = SimulatorROS2Node.CreateSubscription<autoware_control_msgs.msg.Control>(
controlCommandTopic, msg =>
{
// longitudinal
// _adapter.VelocityInput = msg.Longitudinal.Velocity;
// _adapter.IsAccelerationDefinedInput = msg.Longitudinal.Is_defined_acceleration;
// _adapter.AccelerationInput = msg.Longitudinal.Acceleration;
// _adapter.IsJerkDefinedInput = msg.Longitudinal.Is_defined_jerk;
// _adapter.JerkInput = msg.Longitudinal.Jerk;

// lateral
_adapter.SteerAngleInput = -msg.Lateral.Steering_tire_angle * Mathf.Rad2Deg;
// _adapter.IsDefinedSteeringTireRotationRateInput =
// msg.Lateral.Is_defined_steering_tire_rotation_rate;
// _adapter.SteeringTireRotationRateInput = msg.Lateral.Steering_tire_rotation_rate;
}, qos);

_gearCommandSubscriber = SimulatorROS2Node.CreateSubscription<autoware_vehicle_msgs.msg.GearCommand>(
gearCommandTopic, msg => { _adapter.AutomaticShiftInput = Ros2ToVPPUtilities.Ros2ToVPPShift(msg); },
qos);

_vehicleEmergencyStampedSubscriber =
SimulatorROS2Node.CreateSubscription<tier4_vehicle_msgs.msg.VehicleEmergencyStamped>(
vehicleEmergencyStampedTopic, msg => { _adapter.IsEmergencyInput = msg.Emergency; });

_positionSubscriber = SimulatorROS2Node.CreateSubscription<geometry_msgs.msg.PoseWithCovarianceStamped>(
positionTopic, msg =>
{
var positionVector = new Vector3((float)msg.Pose.Pose.Position.X,
(float)msg.Pose.Pose.Position.Y,
(float)msg.Pose.Pose.Position.Z);

var rotationVector = new Quaternion((float)msg.Pose.Pose.Orientation.X,
(float)msg.Pose.Pose.Orientation.Y,
(float)msg.Pose.Pose.Orientation.Z,
(float)msg.Pose.Pose.Orientation.W);

_adapter.PositionInput =
ROS2Utility.RosToUnityPosition(positionVector - Environment.Instance.MgrsOffsetPosition);
_adapter.RotationInput = ROS2Utility.RosToUnityRotation(rotationVector);
_adapter.WillUpdatePositionInput = true;
}, positionQoS);

_actuationCommandSubscriber = SimulatorROS2Node.CreateSubscription<tier4_vehicle_msgs.msg.ActuationCommandStamped>(
actuationCommandTopic, msg =>
{
_adapter.ThrottleInput = msg.Actuation.Accel_cmd;
_adapter.BrakeInput = msg.Actuation.Brake_cmd;
// _adapter.SteerInput = msg.Steer_cmd;
}, actuationQoSSettings);
}

private void OnDestroy()
{
SimulatorROS2Node.RemoveSubscription<autoware_vehicle_msgs.msg.ControlModeReport>(
_controlModeSubscriber);
SimulatorROS2Node.RemoveSubscription<autoware_vehicle_msgs.msg.TurnIndicatorsCommand>(
_turnIndicatorsCommandSubscriber);
SimulatorROS2Node.RemoveSubscription<autoware_vehicle_msgs.msg.HazardLightsCommand>(
_hazardLightsCommandSubscriber);
SimulatorROS2Node.RemoveSubscription<autoware_control_msgs.msg.Control>(_controlCommandSubscriber);
SimulatorROS2Node.RemoveSubscription<autoware_vehicle_msgs.msg.GearCommand>(_gearCommandSubscriber);
SimulatorROS2Node.RemoveSubscription<tier4_vehicle_msgs.msg.VehicleEmergencyStamped>(
_vehicleEmergencyStampedSubscriber);
SimulatorROS2Node.RemoveSubscription<geometry_msgs.msg.PoseWithCovarianceStamped>(_positionSubscriber);
SimulatorROS2Node.RemoveSubscription<tier4_vehicle_msgs.msg.ActuationCommandStamped>(_actuationCommandSubscriber);
}
}
}
112 changes: 112 additions & 0 deletions Assets/AWSIM/Scripts/Vehicles/VPP Integration/Ros2ToVPPUtilities.cs
Original file line number Diff line number Diff line change
@@ -0,0 +1,112 @@
using AWSIM.Scripts.Vehicles.VPP_Integration.Enums;
using VehiclePhysics;

namespace AWSIM.Scripts.Vehicles.VPP_Integration
{
public static class Ros2ToVPPUtilities
{
public static Gearbox.AutomaticGear Ros2ToVPPShift(autoware_vehicle_msgs.msg.GearCommand gearCommand)
{
return gearCommand.Command switch
{
autoware_vehicle_msgs.msg.GearReport.NONE or autoware_vehicle_msgs.msg.GearReport.PARK => Gearbox
.AutomaticGear.P,
autoware_vehicle_msgs.msg.GearReport.REVERSE => Gearbox.AutomaticGear.R,
autoware_vehicle_msgs.msg.GearReport.NEUTRAL => Gearbox.AutomaticGear.N,
autoware_vehicle_msgs.msg.GearReport.DRIVE or autoware_vehicle_msgs.msg.GearReport.LOW => Gearbox
.AutomaticGear.D,
_ => Gearbox.AutomaticGear.P
};
}

public static byte VPPToRos2Shift(int shift)
{
return shift switch
{
1 => autoware_vehicle_msgs.msg.GearReport.PARK,
2 => autoware_vehicle_msgs.msg.GearReport.REVERSE,
3 => autoware_vehicle_msgs.msg.GearReport.NEUTRAL,
4 or 5 or 6 or 7 or 8 => autoware_vehicle_msgs.msg.GearReport.DRIVE,
_ => autoware_vehicle_msgs.msg.GearReport.PARK
};
}

// Turn Signal conversion
public static VPPSignal Ros2ToVPPTurnSignal(
autoware_vehicle_msgs.msg.TurnIndicatorsCommand turnIndicatorsCommand)
{
return turnIndicatorsCommand.Command switch
{
autoware_vehicle_msgs.msg.TurnIndicatorsCommand.DISABLE => VPPSignal.None,
autoware_vehicle_msgs.msg.TurnIndicatorsCommand.ENABLE_LEFT => VPPSignal.Left,
autoware_vehicle_msgs.msg.TurnIndicatorsCommand.ENABLE_RIGHT => VPPSignal.Right,
_ => VPPSignal.None
};
}

public static byte VPPToRos2TurnSignal(VPPSignal turnSignal)
{
return turnSignal switch
{
VPPSignal.None => autoware_vehicle_msgs.msg.TurnIndicatorsReport.DISABLE,
VPPSignal.Left => autoware_vehicle_msgs.msg.TurnIndicatorsReport.ENABLE_LEFT,
VPPSignal.Right => autoware_vehicle_msgs.msg.TurnIndicatorsReport.ENABLE_RIGHT,
_ => autoware_vehicle_msgs.msg.TurnIndicatorsReport.DISABLE
};
}

// Hazard Lights conversion
public static VPPSignal Ros2ToVPPHazard(
autoware_vehicle_msgs.msg.HazardLightsCommand hazardLightsCommand)
{
return hazardLightsCommand.Command switch
{
autoware_vehicle_msgs.msg.HazardLightsCommand.ENABLE => VPPSignal.Hazard,
autoware_vehicle_msgs.msg.HazardLightsCommand.DISABLE => VPPSignal.None,
_ => VPPSignal.None
};
}

public static byte VPPToRos2Hazard(VPPSignal turnSignal)
{
return turnSignal switch
{
VPPSignal.Hazard => autoware_vehicle_msgs.msg.HazardLightsReport.ENABLE,
VPPSignal.None => autoware_vehicle_msgs.msg.HazardLightsReport.DISABLE,
_ => autoware_vehicle_msgs.msg.HazardLightsReport.DISABLE
};
}

// Control Mode conversion
public static VPPControlMode Ros2ToVPPControlMode(
autoware_vehicle_msgs.msg.ControlModeReport controlMode)
{
return controlMode.Mode switch
{
autoware_vehicle_msgs.msg.ControlModeReport.NO_COMMAND => VPPControlMode.NoCommand,
autoware_vehicle_msgs.msg.ControlModeReport.AUTONOMOUS => VPPControlMode.Autonomous,
autoware_vehicle_msgs.msg.ControlModeReport.AUTONOMOUS_STEER_ONLY => VPPControlMode.AutonomousSteerOnly,
autoware_vehicle_msgs.msg.ControlModeReport.AUTONOMOUS_VELOCITY_ONLY => VPPControlMode.AutonomousVelocityOnly,
autoware_vehicle_msgs.msg.ControlModeReport.MANUAL => VPPControlMode.Manual,
autoware_vehicle_msgs.msg.ControlModeReport.DISENGAGED => VPPControlMode.Disengaged,
autoware_vehicle_msgs.msg.ControlModeReport.NOT_READY => VPPControlMode.NotReady,
_ => VPPControlMode.NoCommand
};
}

public static byte VPPToRos2ControlMode(VPPControlMode controlMode)
{
return controlMode switch
{
VPPControlMode.NoCommand => autoware_vehicle_msgs.msg.ControlModeReport.NO_COMMAND,
VPPControlMode.Autonomous => autoware_vehicle_msgs.msg.ControlModeReport.AUTONOMOUS,
VPPControlMode.AutonomousSteerOnly => autoware_vehicle_msgs.msg.ControlModeReport.AUTONOMOUS_STEER_ONLY,
VPPControlMode.AutonomousVelocityOnly => autoware_vehicle_msgs.msg.ControlModeReport.AUTONOMOUS_VELOCITY_ONLY,
VPPControlMode.Manual => autoware_vehicle_msgs.msg.ControlModeReport.MANUAL,
VPPControlMode.Disengaged => autoware_vehicle_msgs.msg.ControlModeReport.DISENGAGED,
VPPControlMode.NotReady => autoware_vehicle_msgs.msg.ControlModeReport.NOT_READY,
_ => autoware_vehicle_msgs.msg.ControlModeReport.NO_COMMAND
};
}
}
}
186 changes: 186 additions & 0 deletions Assets/AWSIM/Scripts/Vehicles/VPP Integration/VPPToRos2Publisher.cs
Original file line number Diff line number Diff line change
@@ -0,0 +1,186 @@
using System.Collections;
using ROS2;
using UnityEngine;

namespace AWSIM.Scripts.Vehicles.VPP_Integration
{
public class VPPToRos2Publisher : MonoBehaviour
{
[Header("AutowareVPPAdapter")]
[SerializeField]
private AutowareVPPAdapter _adapter;

[Header("Ros2 fields")]
[SerializeField]
private string _controlModeReportTopic = "/vehicle/status/control_mode";

[SerializeField] private string _gearReportTopic = "/vehicle/status/gear_status";
[SerializeField] private string _steeringReportTopic = "/vehicle/status/steering_status";
[SerializeField] private string _turnIndicatorsReportTopic = "/vehicle/status/turn_indicators_status";
[SerializeField] private string _hazardLightsReportTopic = "/vehicle/status/hazard_lights_status";
[SerializeField] private string _velocityReportTopic = "/vehicle/status/velocity_status";
[SerializeField] private string _ActuationStatusTopic = "/vehicle/status/actuation_status";
[SerializeField] private string _frameId = "base_link";

[Range(1, 60)][SerializeField] private int _publishHz = 30;
[SerializeField] private QoSSettings _qosSettings;

// messages
private autoware_vehicle_msgs.msg.ControlModeReport _controlModeReportMsg;
private autoware_vehicle_msgs.msg.GearReport _gearReportMsg;
private autoware_vehicle_msgs.msg.SteeringReport _steeringReportMsg;
private autoware_vehicle_msgs.msg.TurnIndicatorsReport _turnIndicatorsReportMsg;
private autoware_vehicle_msgs.msg.HazardLightsReport _hazardLightsReportMsg;
private autoware_vehicle_msgs.msg.VelocityReport _velocityReportMsg;
private tier4_vehicle_msgs.msg.ActuationStatusStamped _actuationStatusReport;

// publisher
private IPublisher<autoware_vehicle_msgs.msg.ControlModeReport> _controlModeReportPublisher;
private IPublisher<autoware_vehicle_msgs.msg.GearReport> _gearReportPublisher;
private IPublisher<autoware_vehicle_msgs.msg.SteeringReport> _steeringReportPublisher;
private IPublisher<autoware_vehicle_msgs.msg.TurnIndicatorsReport> _turnIndicatorsReportPublisher;
private IPublisher<autoware_vehicle_msgs.msg.HazardLightsReport> _hazardLightsReportPublisher;
private IPublisher<autoware_vehicle_msgs.msg.VelocityReport> _velocityReportPublisher;
private IPublisher<tier4_vehicle_msgs.msg.ActuationStatusStamped> _actuationStatusPublisher;

private bool _isInitialized;

private void Start()
{
InitializePublishers();
InitializeMessages();
_isInitialized = true;
if (_isInitialized)
{
StartCoroutine(PublishRoutine());
}
}

private IEnumerator PublishRoutine()
{
var interval = 1.0f / _publishHz;
interval -= 0.00001f; // Allow for accuracy errors.
while (true)
{
UpdateMessages();
PublishMessages();
yield return new WaitForSeconds(interval);
}
}

private void InitializePublishers()
{
var qos = _qosSettings.GetQoSProfile();
_controlModeReportPublisher =
SimulatorROS2Node.CreatePublisher<autoware_vehicle_msgs.msg.ControlModeReport>(_controlModeReportTopic,
qos);
_gearReportPublisher =
SimulatorROS2Node.CreatePublisher<autoware_vehicle_msgs.msg.GearReport>(_gearReportTopic, qos);
_steeringReportPublisher =
SimulatorROS2Node.CreatePublisher<autoware_vehicle_msgs.msg.SteeringReport>(_steeringReportTopic, qos);
_turnIndicatorsReportPublisher =
SimulatorROS2Node.CreatePublisher<autoware_vehicle_msgs.msg.TurnIndicatorsReport>(
_turnIndicatorsReportTopic, qos);
_hazardLightsReportPublisher =
SimulatorROS2Node.CreatePublisher<autoware_vehicle_msgs.msg.HazardLightsReport>(
_hazardLightsReportTopic, qos);
_velocityReportPublisher =
SimulatorROS2Node.CreatePublisher<autoware_vehicle_msgs.msg.VelocityReport>(_velocityReportTopic, qos);
_actuationStatusPublisher =
SimulatorROS2Node.CreatePublisher<tier4_vehicle_msgs.msg.ActuationStatusStamped>(_ActuationStatusTopic,
qos);
}

private void InitializeMessages()
{
_controlModeReportMsg = new autoware_vehicle_msgs.msg.ControlModeReport();
_gearReportMsg = new autoware_vehicle_msgs.msg.GearReport();
_steeringReportMsg = new autoware_vehicle_msgs.msg.SteeringReport();
_turnIndicatorsReportMsg = new autoware_vehicle_msgs.msg.TurnIndicatorsReport();
_hazardLightsReportMsg = new autoware_vehicle_msgs.msg.HazardLightsReport();
_velocityReportMsg = new autoware_vehicle_msgs.msg.VelocityReport
{
Header = new std_msgs.msg.Header
{
Frame_id = _frameId,
}
};
_actuationStatusReport = new tier4_vehicle_msgs.msg.ActuationStatusStamped
{
Header = new std_msgs.msg.Header()
{
Frame_id = _frameId,
}
};
}

private void UpdateMessages()
{
// ControlModeReport
_controlModeReportMsg.Mode = Ros2ToVPPUtilities.VPPToRos2ControlMode(_adapter.VpControlModeReport);

// GearReport
_gearReportMsg.Report = Ros2ToVPPUtilities.VPPToRos2Shift(_adapter.VpGearReport);

// SteeringReport
_steeringReportMsg.Steering_tire_angle = -1 * _adapter.VpSteeringReport * Mathf.Deg2Rad;

// TurnIndicatorsReport
_turnIndicatorsReportMsg.Report = Ros2ToVPPUtilities.VPPToRos2TurnSignal(_adapter.VehicleSignalInput);

// HazardLightsReport
_hazardLightsReportMsg.Report = Ros2ToVPPUtilities.VPPToRos2Hazard(_adapter.VehicleSignalInput);

// VelocityReport
var rosLinearVelocity = ROS2Utility.UnityToRosPosition(_adapter.VpVelocityReport);
var rosAngularVelocity = ROS2Utility.UnityToRosPosition(_adapter.VpAngularVelocityReport);
_velocityReportMsg.Longitudinal_velocity = rosLinearVelocity.x;
_velocityReportMsg.Lateral_velocity = rosLinearVelocity.y;
_velocityReportMsg.Heading_rate = rosAngularVelocity.z;

// ActuationStatusReport
_actuationStatusReport.Status.Accel_status = _adapter.VpThrottleStatusReport;
_actuationStatusReport.Status.Brake_status = _adapter.VpBrakeStatusReport;
_actuationStatusReport.Status.Steer_status = _adapter.VpSteerStatusReport;

// Update timestamps
var time = SimulatorROS2Node.GetCurrentRosTime();
_controlModeReportMsg.Stamp = time;
_gearReportMsg.Stamp = time;
_steeringReportMsg.Stamp = time;
_turnIndicatorsReportMsg.Stamp = time;
_hazardLightsReportMsg.Stamp = time;

MessageWithHeader velocityReportMsgHeader = _velocityReportMsg;
SimulatorROS2Node.UpdateROSTimestamp(ref velocityReportMsgHeader);
MessageWithHeader actuationStatusReportHeader = _actuationStatusReport;
SimulatorROS2Node.UpdateROSTimestamp(ref actuationStatusReportHeader);
}

private void PublishMessages()
{
_controlModeReportPublisher.Publish(_controlModeReportMsg);
_gearReportPublisher.Publish(_gearReportMsg);
_steeringReportPublisher.Publish(_steeringReportMsg);
_turnIndicatorsReportPublisher.Publish(_turnIndicatorsReportMsg);
_hazardLightsReportPublisher.Publish(_hazardLightsReportMsg);
_velocityReportPublisher.Publish(_velocityReportMsg);
_actuationStatusPublisher.Publish(_actuationStatusReport);
}

private void OnDestroy()
{
StopCoroutine(PublishRoutine());

SimulatorROS2Node.RemovePublisher<autoware_vehicle_msgs.msg.ControlModeReport>(_controlModeReportPublisher);
SimulatorROS2Node.RemovePublisher<autoware_vehicle_msgs.msg.GearReport>(_gearReportPublisher);
SimulatorROS2Node.RemovePublisher<autoware_vehicle_msgs.msg.SteeringReport>(_steeringReportPublisher);
SimulatorROS2Node.RemovePublisher<autoware_vehicle_msgs.msg.TurnIndicatorsReport>(
_turnIndicatorsReportPublisher);
SimulatorROS2Node.RemovePublisher<autoware_vehicle_msgs.msg.HazardLightsReport>(
_hazardLightsReportPublisher);
SimulatorROS2Node.RemovePublisher<autoware_vehicle_msgs.msg.VelocityReport>(_velocityReportPublisher);
SimulatorROS2Node.RemovePublisher<tier4_vehicle_msgs.msg.ActuationStatusStamped>(_actuationStatusPublisher);
}
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,151 @@
using AWSIM.Scripts.Vehicles.VPP_Integration.Enums;
using UnityEngine;
using UnityEngine.Serialization;
using VehiclePhysics;

namespace AWSIM.Scripts.Vehicles.VPP_Integration
{
// Handle the signals for the VPP vehicle

[RequireComponent(typeof(AutowareVPPAdapter))]
[RequireComponent(typeof(VPVehicleController))]
public class VPPVehicleSignalHandler : MonoBehaviour
{
[SerializeField] private AutowareVPPAdapter _adapter;
[SerializeField] private VPVehicleController _vpVehicle;

[Header("BrakeLight")]
[SerializeField]
private VehicleVisualEffect.EmissionMaterial[] brakeLights;

[Header("TurnSignal")]
[SerializeField]
private VehicleVisualEffect.EmissionMaterial[] leftTurnSignalLights;

[SerializeField] private VehicleVisualEffect.EmissionMaterial[] rightTurnSignalLights;
[SerializeField] private float _turnSignalTimerIntervalSec = 0.5f;
private float _turnSignalTimer;
private bool _turnSignalOn;

[Header("ReverseLight")]
[SerializeField]
private VehicleVisualEffect.EmissionMaterial[] _reverseLights;

private void Reset()
{
if (_adapter == null)
_adapter = GetComponent<AutowareVPPAdapter>();
if (_vpVehicle == null)
_vpVehicle = GetComponent<VPVehicleController>();
}

private void Start()
{
foreach (var e in brakeLights) e.Initialize();
foreach (var e in leftTurnSignalLights) e.Initialize();
foreach (var e in rightTurnSignalLights) e.Initialize();
foreach (var e in _reverseLights) e.Initialize();
}

private void Update()
{
// User input for turn signal.
if (Input.GetKey(KeyCode.Alpha1))
_adapter.VehicleSignalInput = VPPSignal.Left;
else if (Input.GetKey(KeyCode.Alpha2))
_adapter.VehicleSignalInput = VPPSignal.Right;
else if (Input.GetKey(KeyCode.Alpha3))
_adapter.VehicleSignalInput = VPPSignal.Hazard;
else if (Input.GetKey(KeyCode.Alpha4))
_adapter.VehicleSignalInput = VPPSignal.None;

// brake light.
var isBrakeLight = IsBrakeLight();
ApplyLights(brakeLights, isBrakeLight);

// reverse light.
var isReverseLight = IsReverseLight();
ApplyLights(_reverseLights, isReverseLight);

// User input for brake & reverse light.
if (Input.GetKey(KeyCode.S) || Input.GetKey(KeyCode.Space))
{
ApplyLights(brakeLights, true);
}

if (_vpVehicle.data.bus[Channel.Vehicle][VehicleData.GearboxMode] == (int)Gearbox.AutomaticGear.R)
{
ApplyLights(brakeLights, true);
ApplyLights(_reverseLights, true);
}

// turn signal light.
if (IsTurnSignalOn() == false)
{
_turnSignalTimer = 0;
_turnSignalOn = false;
ApplyLights(leftTurnSignalLights, false);
ApplyLights(rightTurnSignalLights, false);
return;
}

_turnSignalTimer -= Time.deltaTime;
if (_turnSignalTimer < 0f)
{
_turnSignalTimer = _turnSignalTimerIntervalSec;
_turnSignalOn = !_turnSignalOn;
}

var isTurnLeftLight = IsTurnLeftLight();
ApplyLights(leftTurnSignalLights, isTurnLeftLight);

var isTurnRightLight = IsTurnRightLight();
ApplyLights(rightTurnSignalLights, isTurnRightLight);
}

private bool IsBrakeLight()
{
return (_adapter.AutomaticShiftInput == Gearbox.AutomaticGear.D && _adapter.CurrentSpeed < 0)
|| (_adapter.AutomaticShiftInput == Gearbox.AutomaticGear.R && _adapter.CurrentSpeed < 0);
}

private bool IsReverseLight()
{
return _adapter.AutomaticShiftInput == Gearbox.AutomaticGear.R;
}

private bool IsTurnSignalOn()
{
return _adapter.VehicleSignalInput is
VPPSignal.Left or VPPSignal.Right or VPPSignal.Hazard;
}

private bool IsTurnLeftLight()
{
return _adapter.VehicleSignalInput is
VPPSignal.Left or VPPSignal.Hazard && _turnSignalOn;
}

private bool IsTurnRightLight()
{
return _adapter.VehicleSignalInput is
VPPSignal.Right or VPPSignal.Hazard && _turnSignalOn;
}

private static void ApplyLights(VehicleVisualEffect.EmissionMaterial[] emissionMaterials, bool isOn)
{
foreach (var e in emissionMaterials)
{
e.Set(isOn);
}
}

private void OnDestroy()
{
foreach (var e in brakeLights) e.Destroy();
foreach (var e in leftTurnSignalLights) e.Destroy();
foreach (var e in rightTurnSignalLights) e.Destroy();
foreach (var e in _reverseLights) e.Destroy();
}
}
}
59 changes: 33 additions & 26 deletions Assets/AWSIM/Scripts/Vehicles/VehicleVisualEffect.cs
Original file line number Diff line number Diff line change
@@ -11,66 +11,73 @@ namespace AWSIM
/// </summary>

//TODO: Implement proper lighting system for vehicle.

[RequireComponent(typeof(Vehicle))]
public class VehicleVisualEffect : MonoBehaviour
{
[Serializable]
public class EmissionMaterial
{
[SerializeField] MeshRenderer meshRenderer;
[SerializeField] int materialIndex;
[SerializeField] Color lightingColor;
[SerializeField] float emissionIntensity;
[SerializeField] private MeshRenderer meshRenderer;
[SerializeField] private int materialIndex;

[ColorUsage(false, true)]
[SerializeField]
private Color lightingColor;

Material material = null;
private Color defaultEmissionColor;
[SerializeField] private float emissionIntensity;

const string EmissionColor = "_EmissionColor";
private Material _material;
private Color _defaultEmissionColor;
private const string EmissionColor = "_EmissionColor";

public void Initialize()
{
if (material == null)
if (_material == null)
{
material = meshRenderer.materials[materialIndex];
material.EnableKeyword("_EMISSION");
defaultEmissionColor = Color.black;
_material = meshRenderer.materials[materialIndex];
_material.EnableKeyword("_EMISSION");
_defaultEmissionColor = Color.black;
}
}

public void Set(bool isLightOn)
{
if (isLightOn)
{
material.SetColor(EmissionColor, lightingColor * emissionIntensity);
// _material.SetColor(EmissionColor, lightingColor * emissionIntensity);
_material.SetColor(EmissionColor, lightingColor);
}
else
{
material.SetColor(EmissionColor, defaultEmissionColor);
_material.SetColor(EmissionColor, _defaultEmissionColor);
}
}

public void Destroy()
{
if (material != null)
UnityEngine.Object.Destroy(material);
if (_material != null)
UnityEngine.Object.Destroy(_material);
}
}

[SerializeField] Vehicle vehicle;

[Header("BrakeLight")]
[SerializeField] EmissionMaterial[] brakeLights;
[SerializeField]
EmissionMaterial[] brakeLights;

[Header("TurnSignal")]
[SerializeField] EmissionMaterial[] leftTurnSignalLights;
[SerializeField]
EmissionMaterial[] leftTurnSignalLights;

[SerializeField] EmissionMaterial[] rightTurnSignalLights;
[SerializeField] float turnSignalTimerIntervalSec = 0.5f;
float turnSignalTimer = 0;
bool turnSignalOn = false;

[Header("ReverseLight")]
[SerializeField] EmissionMaterial[] reverseLights;
[SerializeField]
EmissionMaterial[] reverseLights;

void Reset()
{
@@ -131,7 +138,7 @@ void Update()
bool IsBrakeLight()
{
return (vehicle.AutomaticShiftInput == Vehicle.Shift.DRIVE && vehicle.AccelerationInput < 0)
|| (vehicle.AutomaticShiftInput == Vehicle.Shift.REVERSE && vehicle.AccelerationInput < 0);
|| (vehicle.AutomaticShiftInput == Vehicle.Shift.REVERSE && vehicle.AccelerationInput < 0);
}

bool IsReverseLight()
@@ -142,22 +149,22 @@ bool IsReverseLight()
bool IsTurnSignalOn()
{
return ((vehicle.SignalInput == Vehicle.TurnSignal.LEFT)
|| ((vehicle.SignalInput == Vehicle.TurnSignal.RIGHT)
|| (vehicle.SignalInput == Vehicle.TurnSignal.HAZARD)));
|| ((vehicle.SignalInput == Vehicle.TurnSignal.RIGHT)
|| (vehicle.SignalInput == Vehicle.TurnSignal.HAZARD)));
}

bool IsTurnLeftLight()
{
return ((vehicle.SignalInput == Vehicle.TurnSignal.LEFT)
|| (vehicle.SignalInput == Vehicle.TurnSignal.HAZARD))
&& turnSignalOn;
|| (vehicle.SignalInput == Vehicle.TurnSignal.HAZARD))
&& turnSignalOn;
}

bool IsTurnRightLight()
{
return ((vehicle.SignalInput == Vehicle.TurnSignal.RIGHT)
|| (vehicle.SignalInput == Vehicle.TurnSignal.HAZARD))
&& turnSignalOn;
|| (vehicle.SignalInput == Vehicle.TurnSignal.HAZARD))
&& turnSignalOn;
}

void ApplyLights(EmissionMaterial[] emissionMaterials, bool isOn)
8 changes: 8 additions & 0 deletions Assets/Vehicle Physics Pro.meta
7 changes: 7 additions & 0 deletions Assets/Vehicle Physics Pro/.gitignore
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
# Ignore everything except directories
*
!*/

# But not these files...
!.gitignore
!/Editor/csc.rsp*
1 change: 1 addition & 0 deletions Assets/Vehicle Physics Pro/Editor/csc.rsp
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
-warn:0
7 changes: 7 additions & 0 deletions Assets/Vehicle Physics Pro/Editor/csc.rsp.meta
12 changes: 6 additions & 6 deletions ProjectSettings/DynamicsManager.asset
Original file line number Diff line number Diff line change
@@ -9,23 +9,23 @@ PhysicsManager:
m_BounceThreshold: 2
m_DefaultMaxDepenetrationVelocity: 10
m_SleepThreshold: 0.005
m_DefaultContactOffset: 0.01
m_DefaultContactOffset: 0.02
m_DefaultSolverIterations: 6
m_DefaultSolverVelocityIterations: 1
m_QueriesHitBackfaces: 0
m_QueriesHitTriggers: 1
m_EnableAdaptiveForce: 0
m_ClothInterCollisionDistance: 0
m_ClothInterCollisionStiffness: 0
m_ContactsGeneration: 1
m_LayerCollisionMatrix: ffefffffffefffffffefffffffffffffffefffffffefffffbfefffffffffffffffffffffffffffffffffffffffefffff88e7ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff
m_ContactsGeneration: 0
m_LayerCollisionMatrix: fffffdfffffffdfffffffdfffffffffffffffdffffffffffbffffffffffffffffffffffffffffffffffffdffffffffffffffffffffffffffffffffffffffffffffffffffe8fbfdffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff
m_SimulationMode: 0
m_AutoSyncTransforms: 0
m_ReuseCollisionCallbacks: 1
m_AutoSyncTransforms: 1
m_ReuseCollisionCallbacks: 0
m_InvokeCollisionCallbacks: 1
m_ClothInterCollisionSettingsToggle: 0
m_ClothGravity: {x: 0, y: -9.81, z: 0}
m_ContactPairsMode: 0
m_ContactPairsMode: 3
m_BroadphaseType: 0
m_WorldBounds:
m_Center: {x: 0, y: 0, z: 0}
2 changes: 1 addition & 1 deletion ProjectSettings/ProjectSettings.asset
Original file line number Diff line number Diff line change
@@ -141,7 +141,7 @@ PlayerSettings:
loadStoreDebugModeEnabled: 0
visionOSBundleVersion: 1.0
tvOSBundleVersion: 1.0
bundleVersion: 1.3.1
bundleVersion: 1.4.0
preloadedAssets: []
metroInputSource: 0
wsaTransparentSwapchain: 0
8 changes: 4 additions & 4 deletions ProjectSettings/QualitySettings.asset
Original file line number Diff line number Diff line change
@@ -4,7 +4,7 @@
QualitySettings:
m_ObjectHideFlags: 0
serializedVersion: 5
m_CurrentQuality: 1
m_CurrentQuality: 3
m_QualitySettings:
- serializedVersion: 3
name: Low
@@ -88,7 +88,7 @@ QualitySettings:
streamingMipmapsRenderersPerFrame: 256
streamingMipmapsMaxLevelReduction: 4
streamingMipmapsMaxFileIORequests: 512
particleRaycastBudget: 2048
particleRaycastBudget: 1024
asyncUploadTimeSlice: 2
asyncUploadBufferSize: 16
asyncUploadPersistentBuffer: 1
@@ -137,7 +137,7 @@ QualitySettings:
streamingMipmapsRenderersPerFrame: 512
streamingMipmapsMaxLevelReduction: 6
streamingMipmapsMaxFileIORequests: 512
particleRaycastBudget: 4096
particleRaycastBudget: 1024
asyncUploadTimeSlice: 2
asyncUploadBufferSize: 16
asyncUploadPersistentBuffer: 1
@@ -186,7 +186,7 @@ QualitySettings:
streamingMipmapsRenderersPerFrame: 512
streamingMipmapsMaxLevelReduction: 7
streamingMipmapsMaxFileIORequests: 1024
particleRaycastBudget: 4096
particleRaycastBudget: 1024
asyncUploadTimeSlice: 2
asyncUploadBufferSize: 16
asyncUploadPersistentBuffer: 1
2 changes: 1 addition & 1 deletion docs/Components/Vehicle/EgoVehicle/index.md
Original file line number Diff line number Diff line change
@@ -7,7 +7,7 @@ The default prefab `EgoVehicle` was developed using a *Lexus RX450h 2015* vehicl
![vehicle](vehicle.png)

!!! tip "Own EgoVehicle prefab"
If you would like to develop your own `EgoVehicle` prefab, we encourage you to read this [tutorial](../../../Components/Vehicle/AddNewVehicle/AddAVehicle/).
If you would like to develop your own `EgoVehicle` prefab, we encourage you to read this [tutorial](../../../Components/Vehicle/AddNewVehicle/AddAVehicle/).

### Supported features

146 changes: 146 additions & 0 deletions docs/Components/Vehicle/VPPIntegration/index.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,146 @@
# VPP Integration with the Ego Vehicle

VPP is a package that provides realistic vehicle simulation. Here you can find a guide on how to integrate VPP with the
Ego Vehicle.

!!! tip "Example Vehicle"
It is recommended to take a look at the default Lexus vehicle prefab as example to see how the components are set up.

## Initial setup

To be able to use VPP with the ego vehicle, please attach these components to the vehicle prefab:
From the VPP package:

Added to prefab root:

- `Vehicle Controller`
- `Standard Input`
- `Camera Target` (Optional, you can use your own implementation)
- `Telemetry Window` (Optional, used to display telemetry data)
- `Visual Effects` (Optional, used to visualize the steering wheel)

!!! warning "Wheel Colliders"
VPP uses its own implementation of the `Wheel Colliders`! For the wheel colliders, please add the `Wheel Colliders`
provided by the VPP package. They share the same name with the default Unity components.

After adding those scripts continue with adding the following components to the vehicle prefab:

Added to prefab root:

- `AutowareVPPAdapter.cs`: Used for applying the control inputs to the vehicle.
- `Ros2ToVPPInput.cs`: Used for receiving the control inputs from the Autoware.
- `VPPVehicleSignalHandler.cs`: Used for handling the signals from the vehicle.
- `BirdEyeView.cs`: Used for providing top-down ortho view.

To be able to report the vehicle state to the Autoware, you'll need this script in the URDF.
_This script is added in the `VehicleStatusSensor` prefab by default._

Added to URDF:

- `VPPToRos2Publisher.cs`: Used for publishing the vehicle state to the Autoware.

## Setting up the components

After doing the initial setup, we will need to configure the components for your vehicle.

Set up the `Vehicle Controller` and the `Wheel Colliders` with the correct values for your vehicle. Using values from
the real vehicles will result in a more realistic simulation.

You can refer to [VPP documentation](https://vehiclephysics.com/components/component-guide/) for more detailed
information about the VPP components.

As for the other components we've added to the vehicle, we have to give their necessary references.

!!! warning "Camera Controller"
If you've added the `Camera Target` component, you'll need a separate gameObject with a `Camera Controller`. Don't
forget to assign vehicle Transform reference in this component. This is used by default in the `AutowareSimulation`
scene.

### AutowareVPPAdapter.cs

| Variable | Description |
|--------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------|
| `Front Wheels` | Used for handling the steering input. Assign the front wheel colliders of the vehicle. |
| `Simulate Steering` | Simulate the steering instead of assigning wheel angles directly. |
| `Steer Wheel Input` | Change applied to steering wheel when simulating steering. Applied every in fixed update as an input to the VPP steering. _(0-100)_ |
| `Emergency Brake Percent` | The amount of brake pedal percent applied when the emergency is triggered. _(0-100)_ |
| `Update Position Offset Y` | The height offset value for the vehicle position when initializing position of the vehicle from the Rviz. _(In meters)_ |
| `Update Position Ray Origin Y` | The height of the raycast origin for the vehicle position when initializing position of the vehicle from the Rviz. _(In meters)_ |
| `Do Pedal Calibration` | Mode for vehicle pedal calibration. If enabled the vehicle pedals can be set to certain percentages with Numpad(-,+,0) keys. Uses %10 increments by default. |

### Ros2ToVPPInput.cs

| Variable | Description |
|-----------|---------------------------------------------------------------------------------------------------------|
| `Adapter` | Reference to the `AutowareVPPAdapter` script. _(Drag the object that has the corresponding component.)_ |
| `Topics` | Topics used in the Ros2 communication. You can change the topics according to your case. |

- `QoS Settings`: Quality of Service settings for the Ros2 communication. Currently, it is as follows:

| Variable | Value |
|----------------------|-----------------|
| `Reliability Policy` | Reliable |
| `Durability Policy` | Transient Local |
| `History Policy` | Keep Last |
| `Depth` | 1 |

- `Position QoS Setttings`: QoS settings for the position topic:

| Variable | Value |
|----------------------|-----------|
| `Reliability Policy` | Reliable |
| `Durability Policy` | Volatile |
| `History Policy` | Keep Last |
| `Depth` | 1 |

- `Actuation QoS Setttings`: QoS settings for the actuation topic:

| Variable | Value |
|----------------------|-----------|
| `Reliability Policy` | Reliable |
| `Durability Policy` | Volatile |
| `History Policy` | Keep Last |
| `Depth` | 1 |

### VehicleSignalHandler.cs

| Variable | Description |
|--------------|---------------------------------------------------------------------------------------------------------|
| `Adapter` | Reference to the `AutowareVPPAdapter` script. _(Drag the object that has the corresponding component.)_ |
| `Vp Vehicle` | Reference to the `Vehicle Controller` script. _(Drag the object that has the corresponding component.)_ |

Rest of the signal related settings are same as `VehicleVisualEffect.cs`. These two scripts are basically same.

### VPPToRos2Publisher.cs

Same as `VehicleStatusSensor.cs` but the `Adapter` reference is the `AutowareVPPAdapter` script.

## Setting up the child objects

For the VPP to work correctly, you need to set up the child objects of the vehicle prefab.

### Setting up the reference for Ackermann Steering:

1. Create an empty game object named `"Ackermann"` and set as the direct child of the prefab. Then assign reference in
the `Vehicle Controller`.
2. Move position of the created game object to the middle of the rear axle and set its height to the bottom of the rear
wheels. Make sure the rotations are `(0,0,0)`

Assign the reference in the `Vehicle Controller`. For details, you can refer to the VPP.

VPP Reference: [Ackermann](https://vehiclephysics.com/blocks/steering/)

### Setting up the reference for Dynamics:

1. Create an empty game object named `"Dynamics"` and set as the direct child of the prefab. Add the following
components to this object:
- `Rolling Friction`
- `Anti-roll Bar` (x2)

2. Create an empty game object named `"Aero"` and set as the child of the "Dynamics". Add the following
components to this object:
- `Aerodynamic Surface`

Assign the references in the `Vehicle Controller`. For configuring these components, you can refer to the VPP documentation.

VPP Reference: [Dynamics](https://vehiclephysics.com/components/vehicle-dynamics/)
28 changes: 28 additions & 0 deletions docs/DeveloperGuide/EditorSetup/VPPCommunityEdition/index.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
# Vehicle Physics Pro Community Edition

## Add VPP-CE from Asset Store

- Go to Unity Asset Store and add VPP-CE to personal assets.

[Vehicle Physics Pro Asset Store link](https://assetstore.unity.com/packages/tools/physics/vehicle-physics-pro-community-edition-153556)

## Add VPP-CE to the Unity Editor

1. Open up the Unity Editor.
2. Go to the `Window` menu and select `Package Manager`.
3. Make sure the `My Assets` tab is selected from the top left of the Package Manager window.
4. Find & select the VPP-CE from the list and click `Download` or `Import` from the bottom left of the Package Manager
window.
5. There will be a popup window showing contents of the package.
- Select `Sdk` folder to import. Rest of the content is optional.
- Click `Import` to add VPP-CE to the project.

## Useful links:

Unity Package manager:

- [Manual](https://docs.unity3d.com/Manual/upm-ui.html)

VPP Website:

- [Vehicle Physics](https://vehiclephysics.com/)
11 changes: 8 additions & 3 deletions docs/GettingStarted/QuickStartDemo/index.md
Original file line number Diff line number Diff line change
@@ -206,15 +206,20 @@ rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
```
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_EXPORT_COMPILE_COMMANDS=1
```
8. Launch Autoware.
8. Provide pedal maps to Autoware.
Copy default Lexus pedal maps to the following location:
Pedal Maps: [Google Drive Link](https://drive.google.com/drive/folders/1S0fcNHzkiF3F03hWqonQW8SYKPb2zQo-?usp=drive_link)
Location: `.../autoware/install/autoware_raw_vehicle_cmd_converter/share/autoware_raw_vehicle_cmd_converter/data/default/`
9. Launch Autoware.
```
source install/setup.bash
ros2 launch autoware_launch e2e_simulator.launch.xml vehicle_model:=sample_vehicle sensor_model:=awsim_labs_sensor_kit map_path:=<absolute path of map folder>
ros2 launch autoware_launch e2e_simulator.launch.xml vehicle_model:=awsim_labs_vehicle sensor_model:=awsim_labs_sensor_kit map_path:=<absolute path of map folder> launch_vehicle_interface:=true

# Use the absolute path for the map folder, don't use the ~ operator.

# Example:
ros2 launch autoware_launch e2e_simulator.launch.xml vehicle_model:=sample_vehicle sensor_model:=awsim_labs_sensor_kit map_path:=/home/your_username/autoware_map/nishishinjuku_autoware_map
ros2 launch autoware_launch e2e_simulator.launch.xml vehicle_model:=awsim_labs_vehicle sensor_model:=awsim_labs_sensor_kit map_path:=/home/your_username/autoware_map/nishishinjuku_autoware_map launch_vehicle_interface:=true
```
![](Image_2.png)
4 changes: 4 additions & 0 deletions docs/GettingStarted/SetupUnityProject/index.md
Original file line number Diff line number Diff line change
@@ -142,6 +142,10 @@ To properly run and use AWSIM project in Unity it is required to download map pa
The Externals directory is added to the `.gitignore` because the map has a large file size and should not be directly uploaded to the repository.
## Import Vehicle Physics Pro Community Edition Asset
Import Vehicle Physics Pro CE by following these instructions: [VPP CE Setup](../../DeveloperGuide/EditorSetup/VPPCommunityEdition/index.md)
## Import Graphy Asset
Import Graphy by following these instructions: [Graphy Asset Setup](../../DeveloperGuide/EditorSetup/Graphy/index.md)
2 changes: 2 additions & 0 deletions mkdocs.yml
Original file line number Diff line number Diff line change
@@ -134,6 +134,7 @@ nav:
- Add Visual Elements: Components/Vehicle/AddNewVehicle/AddVisualElements/index.md
- Customize Slip: Components/Vehicle/CustomizeSlip/index.md
- FollowCamera: Components/Vehicle/FollowCamera/index.md
- VPP Integration: Components/Vehicle/VPPIntegration/index.md
- Sensors:
- LiDAR Sensor:
- LiDAR Sensor: Components/Sensors/LiDARSensor/LiDARSensor/index.md
@@ -180,6 +181,7 @@ nav:
- VSCode Configuration: DeveloperGuide/EditorSetup/VSCode/index.md
- Rider Configuration: DeveloperGuide/EditorSetup/JetBrainsRider/index.md
- Graphy Asset Setup: DeveloperGuide/EditorSetup/Graphy/index.md
- Vehicle Physics Pro CE Setup: DeveloperGuide/EditorSetup/VPPCommunityEdition/index.md
- Trouble shooting: DeveloperGuide/TroubleShooting/index.md
- Documentation: DeveloperGuide/Documentation/index.md
- License : DeveloperGuide/License/index.md