-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathIscp_ControlSubscriber.cs
More file actions
108 lines (95 loc) · 3.49 KB
/
Copy pathIscp_ControlSubscriber.cs
File metadata and controls
108 lines (95 loc) · 3.49 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
using System;
using UnityEngine;
using Newtonsoft.Json.Linq;
/**
* Control DataFormat ( string -> JSON )
* "{
* "steering" : 0.0,
* "accel" : 0.0,
* "footbrake" : 0.0,
* "handbrake" : 0.0
* }"
*/
public class Iscp_ControlSubscriber : MonoBehaviour
{
[SerializeField]
private string dataType = "string/json";
[SerializeField]
private string dataName = "v1/1/control";
[SerializeField]
[IntdashLabel("Node IDiEdge UUIDj")]
private string nodeId = "";
public CarIscpControl TargetComponent;
[SerializeField]
private string receivedTime;
public string ReceivedTime => receivedTime;
[SerializeField]
private string receivedData;
public string ReceivedData => receivedData;
[SerializeField] private float steering;
public float Steering => steering;
[SerializeField] private float accel;
public float Accel => accel;
[SerializeField] private float footbrake;
public float Footbrake => footbrake;
[SerializeField] private float handbrake;
public float Handbrake => handbrake;
// MEMO: Basically, IscpConnection attachment from Inspector is not required, but can be specified.
[SerializeField] private IscpConnection iscp;
private IscpDownstream downstream;
// Called when the script is first activated.
private void Awake()
{
// Setup iSCP.
if (this.iscp == null)
{
this.iscp = IscpConnection.GetOrCreateSharedInstance();
}
// Register downstream.
downstream = this.iscp.RegisterDownstream(nodeId, dataName, dataType, OnReceiveDataPoints);
}
// Received data points events.
private void OnReceiveDataPoints(DateTime baseTime, iSCP.Model.DataPointGroup[] dataPointGroups)
{
foreach (var group in dataPointGroups)
{
foreach (var d in group.DataPoints)
{
Dispatcher.RunOnMainThread(() =>
{
try
{
if (this == null) return;
if (!this.enabled) return;
receivedTime = DateTime.UtcNow.ToLocalTime().ToString("HH:mm:ss.ffffff");
// Extract the necessary data from the data format.
var controlData = System.Text.Encoding.UTF8.GetString(d.Payload);
receivedData = controlData;
// Json Parse
{
JObject json = JObject.Parse(controlData);
steering = json["steering"].ToObject<float>();
accel = json["accel"].ToObject<float>();
footbrake = json["footbrake"].ToObject<float>();
handbrake = json["handbrake"].ToObject<float>();
}
// Move Vehicles
if (TargetComponent != null)
{
if (TargetComponent.enabled)
{
TargetComponent.Move(steering, accel, footbrake, handbrake);
}
}
}
catch (Exception e)
{
Debug.LogError("Failed to deserialize control message. " + e.Message);
}
});
}
}
}
private void OnEnable() { }
private void OnDisable() { }
}