forked from KIT-ISAS/iviz
-
Notifications
You must be signed in to change notification settings - Fork 0
/
ReceiverConnector.cs
199 lines (169 loc) · 6.78 KB
/
ReceiverConnector.cs
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
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
using System;
using System.Net;
using System.Net.Sockets;
using System.Threading;
using System.Threading.Tasks;
using Iviz.Roslib.XmlRpc;
using Iviz.Tools;
using Iviz.XmlRpc;
namespace Iviz.Roslib;
/// <summary>
/// Class in charge of trying to connect and reconnect to a ROS publisher
/// </summary>
internal class ReceiverConnector
{
const int MaxConnectionRetries = 120;
const int DisposeTimeoutInMs = 2000;
readonly RosNodeClient client;
readonly string topic;
readonly CancellationTokenSource tokenSource = new();
readonly Task task;
readonly Action<ReceiverConnector, ReceiverConnectorResponse> onConnectionSucceeded;
readonly RpcUdpTopicRequest? udpRequest;
readonly UdpClient? udpClient;
readonly RosTransportHint transportHint;
ReceiverStatus status;
bool disposed;
CancellationToken Token => tokenSource.Token;
public Uri RemoteUri => client.Uri;
public ErrorMessage? ErrorDescription { get; private set; }
public bool IsAlive => !task.IsCompleted;
public ReceiverConnector(RosNodeClient client, string topic, RosTransportHint transportHint,
RpcUdpTopicRequest? udpRequest, Action<ReceiverConnector, ReceiverConnectorResponse> onConnectionSucceeded)
{
this.topic = topic;
this.client = client;
this.onConnectionSucceeded = onConnectionSucceeded;
this.transportHint = transportHint;
if (udpRequest != null)
{
udpClient = new UdpClient(AddressFamily.InterNetworkV6) { Client = { DualMode = true } };
udpClient.Client.Bind(new IPEndPoint(IPAddress.IPv6Any, 0));
int port = ((IPEndPoint)udpClient.Client.LocalEndPoint!).Port;
this.udpRequest = udpRequest.WithPort(port);
}
status = ReceiverStatus.ConnectingRpc;
task = TaskUtils.Run(async () => await KeepReconnecting().AwaitNoThrow(this));
}
async ValueTask KeepReconnecting()
{
int numTry;
for (numTry = 0; numTry < MaxConnectionRetries; numTry++)
{
Logger.LogDebugFormat("{0}: Attempting connection...", this);
var response = await RequestTopic();
if (response != null)
{
if (!response.IsValid)
{
ErrorDescription = new ErrorMessage(response.StatusMessage);
break;
}
if (response.IsFailure)
{
ErrorDescription = new ErrorMessage(response.StatusMessage);
break;
}
if (response.TcpResponse is { Port: <= 0 or > ushort.MaxValue })
{
ErrorDescription = new ErrorMessage(
$"Received invalid TCP port {response.TcpResponse.Value.Port.ToString()}");
break;
}
if (response.UdpResponse is { Port: <= 0 or > ushort.MaxValue })
{
ErrorDescription = new ErrorMessage(
$"Received invalid UDP port {response.UdpResponse.Port.ToString()}");
break;
}
if (response.UdpResponse is { MaxPacketSize: <= UdpRosParams.HeaderLength or > ushort.MaxValue })
{
ErrorDescription = new ErrorMessage(
$"Received invalid max packet size {response.UdpResponse.MaxPacketSize.ToString()}");
break;
}
ErrorDescription = null;
status = ReceiverStatus.Connected;
Logger.LogDebugFormat("{0}: Connection succeeded.", this);
onConnectionSucceeded(this,
new ReceiverConnectorResponse(response.TcpResponse, response.UdpResponse, udpClient));
return;
}
Logger.LogDebugFormat("{0}: Connection failed! Reason: {1}", this, ErrorDescription);
try
{
await Task.Delay(WaitTimeInMsFromTry(numTry), Token);
}
catch (OperationCanceledException)
{
status = ReceiverStatus.Canceled;
udpClient?.Dispose();
return;
}
}
if (numTry == MaxConnectionRetries)
{
status = ReceiverStatus.OutOfRetries;
ErrorDescription = new ErrorMessage($"Giving up after {MaxConnectionRetries.ToString()} tries");
}
else
{
status = ReceiverStatus.Dead;
}
Logger.LogDebugFormat("{0}: Removing connection - {1}", this, ErrorDescription);
udpClient?.Dispose();
}
async ValueTask<RequestTopicResponse?> RequestTopic()
{
try
{
return await client.RequestTopicAsync(topic, transportHint, udpRequest, Token);
}
catch (Exception e)
{
ErrorDescription = e switch
{
OperationCanceledException => null,
ObjectDisposedException => null,
XmlRpcException => new ErrorMessage(e.InnerException ?? e),
_ => new ErrorMessage(e)
};
}
return null;
}
public async ValueTask DisposeAsync(CancellationToken token)
{
if (disposed)
{
return;
}
disposed = true;
tokenSource.Cancel();
await task.AwaitNoThrow(DisposeTimeoutInMs, this, token);
Logger.LogDebugFormat("{0}: Disposing!", this);
}
public SubscriberReceiverState State => new UninitializedReceiverState(RemoteUri)
{
Status = status,
ErrorDescription = ErrorDescription
};
public override string ToString() => $"[{nameof(ReceiverConnector)} '{topic}' Uri='{RemoteUri}']";
static int WaitTimeInMsFromTry(int numTry) => BaseUtils.Random.Next(0, 1000) +
numTry switch
{
< 5 => 3000,
< 50 => 5000,
_ => 10000
};
}
public sealed class ReceiverConnectorResponse
{
public Endpoint? TcpResponse { get; }
public RpcUdpTopicResponse? UdpResponse { get; }
public UdpClient? UdpClient { get; }
public ReceiverConnectorResponse(Endpoint? tcpResponse, RpcUdpTopicResponse? udpResponse, UdpClient? udpClient) =>
(TcpResponse, UdpResponse, UdpClient) = (tcpResponse, udpResponse, udpClient);
public void Deconstruct(out Endpoint? tcpResponse, out RpcUdpTopicResponse? udpResponse,
out UdpClient? udpClient) =>
(tcpResponse, udpResponse, udpClient) = (TcpResponse, UdpResponse, UdpClient);
}