From 177750aac3fb4bf605651260f9b7081846adef28 Mon Sep 17 00:00:00 2001 From: David Uhm Date: Thu, 23 Apr 2020 17:28:36 -0700 Subject: [PATCH] Initial commit --- LaneFollowingSensor.cs | 74 +++++++++++++++++++++++++++++++++ LaneFollowingSensor.cs.meta | 11 +++++ LaneFollowingSensor.prefab | 48 +++++++++++++++++++++ LaneFollowingSensor.prefab.meta | 7 ++++ 4 files changed, 140 insertions(+) create mode 100644 LaneFollowingSensor.cs create mode 100644 LaneFollowingSensor.cs.meta create mode 100644 LaneFollowingSensor.prefab create mode 100644 LaneFollowingSensor.prefab.meta diff --git a/LaneFollowingSensor.cs b/LaneFollowingSensor.cs new file mode 100644 index 0000000..f7f1fc1 --- /dev/null +++ b/LaneFollowingSensor.cs @@ -0,0 +1,74 @@ +/** + * Copyright (c) 2020 LG Electronics, Inc. + * + * This software contains code licensed as described in LICENSE. + * + */ + +using UnityEngine; +using Simulator.Bridge; +using Simulator.Utilities; +using Simulator.Sensors.UI; +using Simulator.Bridge.Ros; +using System.Collections.Generic; + +namespace Simulator.Sensors +{ + [SensorType("Lane Following", new System.Type[] { })] + public class LaneFollowingSensor : SensorBase, IVehicleInputs + { + private float ADSteerInput = 0f; + private double LastControlUpdate = 0f; + + public float SteerInput { get; private set; } = 0f; + public float AccelInput { get; private set; } = 0f; + public float BrakeInput { get; private set; } = 0f; + + private void Awake() + { + LastControlUpdate = SimulatorManager.Instance.CurrentTime; + } + + public override void OnBridgeSetup(IBridge bridge) + { + bridge.AddReader(Topic, data => + { + LastControlUpdate = SimulatorManager.Instance.CurrentTime; + ADSteerInput = (float)data.twist.angular.x; + }); + } + + public void Update() + { + if (SimulatorManager.Instance.CurrentTime - LastControlUpdate >= 0.5) + { + ADSteerInput = SteerInput = 0f; + } + } + + private void FixedUpdate() + { + if (SimulatorManager.Instance.CurrentTime - LastControlUpdate < 0.5f) + { + SteerInput = ADSteerInput; + } + } + + public override void OnVisualize(Visualizer visualizer) + { + Debug.Assert(visualizer != null); + var graphData = new Dictionary() + { + {"AD Steer Input", ADSteerInput}, + {"Last Control Update", LastControlUpdate}, + }; + + visualizer.UpdateGraphValues(graphData); + } + + public override void OnVisualizeToggle(bool state) + { + // + } + } +} diff --git a/LaneFollowingSensor.cs.meta b/LaneFollowingSensor.cs.meta new file mode 100644 index 0000000..f535435 --- /dev/null +++ b/LaneFollowingSensor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d82409b4466fbf38eaab962e12c22546 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/LaneFollowingSensor.prefab b/LaneFollowingSensor.prefab new file mode 100644 index 0000000..e2cfe7c --- /dev/null +++ b/LaneFollowingSensor.prefab @@ -0,0 +1,48 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!1 &7421421382082150271 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 2562287667199667010} + - component: {fileID: 5791120669988743586} + m_Layer: 0 + m_Name: LaneFollowingSensor + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &2562287667199667010 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7421421382082150271} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 0} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &5791120669988743586 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7421421382082150271} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d82409b4466fbf38eaab962e12c22546, type: 3} + m_Name: + m_EditorClassIdentifier: + Name: Lane Following + Topic: + Frame: diff --git a/LaneFollowingSensor.prefab.meta b/LaneFollowingSensor.prefab.meta new file mode 100644 index 0000000..dac3cad --- /dev/null +++ b/LaneFollowingSensor.prefab.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 07f7877407174efcbb51c64423332779 +PrefabImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: