Skip to content

Commit fde9ae1

Browse files
ThadHouseThadHouse
committed
Adds NetworkTables settings for CameraServer
1 parent 813be83 commit fde9ae1

File tree

1 file changed

+261
-23
lines changed

1 file changed

+261
-23
lines changed

‎WPILib/CameraServer.cs

+261-23
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22
using System.Collections.Generic;
33
using System.Linq;
44
using System.Text;
5+
using System.Text.RegularExpressions;
56
using System.Threading;
67
using System.Threading.Tasks;
78
using CSCore;
@@ -118,7 +119,7 @@ private List<string> GetSinkStreamValues(int sink)
118119
}
119120
}
120121

121-
private static List<string> GetSourceStreamValues(int source)
122+
private List<string> GetSourceStreamValues(int source)
122123
{
123124
// ignore all but httpcamera
124125
if (NativeMethods.GetSourceKind(source) != SourceKind.Http)
@@ -131,6 +132,23 @@ private static List<string> GetSourceStreamValues(int source)
131132
{
132133
values[i] = $"mjpeg:{values[i]}";
133134
}
135+
136+
lock (m_lockObject)
137+
{
138+
foreach (VideoSink i in m_sinks.Values)
139+
{
140+
int sink = i.Handle;
141+
int sinkSource = NativeMethods.GetSinkSource(sink);
142+
if (source == sinkSource && NativeMethods.GetSinkKind(sink) == SinkKind.Mjpeg)
143+
{
144+
List<string> finalValues = new List<string>(values);
145+
int port = NativeMethods.GetMjpegServerPort(sink);
146+
finalValues.Add(MakeStreamValue("172.22.11.2", port));
147+
return finalValues;
148+
}
149+
}
150+
}
151+
134152
return values;
135153
}
136154

@@ -143,10 +161,13 @@ private void UpdateStreamValues()
143161
int sink = i.Handle;
144162

145163
int source = NativeMethods.GetSinkSource(sink);
164+
if (source == 0) continue;
146165
ITable table;
147166
m_tables.TryGetValue(source, out table);
148167
if (table != null)
149168
{
169+
// Don't set stream values if this is a HttpCamera passthrough
170+
if (NativeMethods.GetSourceKind(source) == SourceKind.Http) continue;
150171
var values = GetSinkStreamValues(sink);
151172
if (values.Count > 0)
152173
{
@@ -173,6 +194,156 @@ private void UpdateStreamValues()
173194
}
174195
}
175196

197+
private static string PixelFormatToString(PixelFormat pixelFormat)
198+
{
199+
switch (pixelFormat)
200+
{
201+
case PixelFormat.Mjpeg:
202+
return "MJPEG";
203+
case PixelFormat.YUYV:
204+
return "YUYV";
205+
case PixelFormat.RGB565:
206+
return "RGB565";
207+
case PixelFormat.BGR:
208+
return "BGR";
209+
case PixelFormat.GRAY:
210+
return "Gray";
211+
default:
212+
return "Unknown";
213+
}
214+
}
215+
216+
private static PixelFormat PixelFormatFromString(string pixelFormatStr)
217+
{
218+
switch (pixelFormatStr)
219+
{
220+
case "MJPEG":
221+
case "mjpeg":
222+
case "JPEG":
223+
case "jpeg":
224+
return PixelFormat.Mjpeg;
225+
case "YUYV":
226+
case "yuyv":
227+
return PixelFormat.YUYV;
228+
case "RGB565":
229+
case "rgb565":
230+
return PixelFormat.RGB565;
231+
case "BGR":
232+
case "bgr":
233+
return PixelFormat.BGR;
234+
case "GRAY":
235+
case "Gray":
236+
case "gray":
237+
return PixelFormat.GRAY;
238+
default:
239+
return PixelFormat.Unknown;
240+
}
241+
}
242+
243+
private const string ReMode = "(?<width>[0-9]+)\\s*x\\s*(?<height>[0-9]+)\\s+(?<format>.*?)\\s+"
244+
+ "(?<fps>[0-9.]+)\\s*fps";
245+
246+
private static readonly Regex m_matcher = new Regex(ReMode);
247+
248+
private static VideoMode VideoModeFromString(string modeStr)
249+
{
250+
var match = m_matcher.Match(modeStr);
251+
if (!match.Success)
252+
{
253+
return new VideoMode(PixelFormat.Unknown, 0, 0, 0);
254+
}
255+
PixelFormat format = PixelFormatFromString(match.Groups["format"].Value);
256+
int width;
257+
int height;
258+
double fps;
259+
if (!int.TryParse(match.Groups["width"].Value, out width))
260+
{
261+
return new VideoMode(PixelFormat.Unknown, 0, 0, 0);
262+
}
263+
if (!int.TryParse(match.Groups["height"].Value, out height))
264+
{
265+
return new VideoMode(PixelFormat.Unknown, 0, 0, 0);
266+
}
267+
if (!double.TryParse(match.Groups["fps"].Value, out fps))
268+
{
269+
return new VideoMode(PixelFormat.Unknown, 0, 0, 0);
270+
}
271+
return new VideoMode(format, width, height, (int)fps);
272+
}
273+
274+
private static string VideoModeToString(VideoMode mode)
275+
{
276+
return $"{mode.Width.ToString()}x{mode.Height.ToString()} {PixelFormatToString(mode.PixelFormat)} {mode.FPS.ToString()} fps";
277+
}
278+
279+
private static List<string> GetSourceModeValues(int sourceHandle)
280+
{
281+
var modes = NativeMethods.EnumerateSourceVideoModes(sourceHandle);
282+
List<string> modeStrings = new List<string>(modes.Count);
283+
foreach (VideoMode videoMode in modes)
284+
{
285+
modeStrings.Add(VideoModeToString(videoMode));
286+
}
287+
return modeStrings;
288+
}
289+
290+
private static void PutSourcePropertyValue(ITable table, VideoEvent evnt, bool isNew)
291+
{
292+
string name;
293+
string infoName;
294+
if (evnt.Name.StartsWith("raw_"))
295+
{
296+
name = $"RawProperty/{evnt.Name.Substring(4)}";
297+
infoName = $"RawPropertyInfo/{evnt.Name.Substring(4)}";
298+
}
299+
else
300+
{
301+
name = $"Property/{evnt.Name}";
302+
infoName = $"PropertyInfo/{evnt.Name}";
303+
}
304+
305+
switch (evnt.PropertyKind)
306+
{
307+
case PropertyKind.Boolean:
308+
if (isNew)
309+
{
310+
table.SetDefaultBoolean(name, evnt.Value != 0);
311+
}
312+
else
313+
{
314+
table.PutBoolean(name, evnt.Value != 0);
315+
}
316+
break;
317+
case PropertyKind.Enum:
318+
case PropertyKind.Integer:
319+
if (isNew)
320+
{
321+
table.SetDefaultNumber(name, evnt.Value);
322+
table.PutNumber($"{infoName}/min", NativeMethods.GetPropertyMin(evnt.PropertyHandle));
323+
table.PutNumber($"{infoName}/max", NativeMethods.GetPropertyMax(evnt.PropertyHandle));
324+
table.PutNumber($"{infoName}/step", NativeMethods.GetPropertyStep(evnt.PropertyHandle));
325+
table.PutNumber($"{infoName}/default", NativeMethods.GetPropertyDefault(evnt.PropertyHandle));
326+
}
327+
else
328+
{
329+
table.PutNumber(name, evnt.Value);
330+
}
331+
break;
332+
case PropertyKind.String:
333+
if (isNew)
334+
{
335+
table.SetDefaultString(name, evnt.ValueStr);
336+
}
337+
else
338+
{
339+
table.PutString(name, evnt.ValueStr);
340+
}
341+
break;
342+
default:
343+
break;
344+
}
345+
}
346+
176347
private CameraServer()
177348
{
178349
m_lockObject = new object();
@@ -200,6 +371,9 @@ private CameraServer()
200371
NativeMethods.GetSourceDescription(vidEvent.SourceHandle));
201372
table.PutBoolean("connected", NativeMethods.IsSourceConnected(vidEvent.SourceHandle));
202373
table.PutStringArray("streams", GetSourceStreamValues(vidEvent.SourceHandle));
374+
VideoMode mode = NativeMethods.GetSourceVideoMode(vidEvent.SourceHandle);
375+
table.SetDefaultString("mode", VideoModeToString(mode));
376+
table.PutStringArray("modes", GetSourceModeValues(vidEvent.SourceHandle));
203377
break;
204378
}
205379
case EventKind.SourceDestroyed:
@@ -209,6 +383,7 @@ private CameraServer()
209383
{
210384
table.PutString("source", "");
211385
table.PutStringArray("streams", new string[0]);
386+
table.PutStringArray("modes", new string[0]);
212387
}
213388
break;
214389
}
@@ -232,43 +407,55 @@ private CameraServer()
232407
}
233408
case EventKind.SourceVideoModesUpdated:
234409
{
410+
ITable table = GetSourceTable(vidEvent.SourceHandle);
411+
if (table != null)
412+
{
413+
table.PutStringArray("modes", GetSourceModeValues(vidEvent.SourceHandle));
414+
}
235415
break;
236416
}
237417
case EventKind.SourceVideoModeChanged:
238418
{
419+
ITable table = GetSourceTable(vidEvent.SourceHandle);
420+
if (table != null)
421+
{
422+
table.PutString("mode", VideoModeToString(vidEvent.Mode));
423+
}
239424
break;
240425
}
241426
case EventKind.SourcePropertyCreated:
242427
{
428+
ITable table = GetSourceTable(vidEvent.SourceHandle);
429+
if (table != null)
430+
{
431+
PutSourcePropertyValue(table, vidEvent, true);
432+
}
243433
break;
244434
}
245435
case EventKind.SourcePropertyValueUpdated:
246436
{
437+
ITable table = GetSourceTable(vidEvent.SourceHandle);
438+
if (table != null)
439+
{
440+
PutSourcePropertyValue(table, vidEvent, false);
441+
}
247442
break;
248443
}
249444
case EventKind.SourcePropertyChoicesUpdated:
250445
{
446+
ITable table = GetSourceTable(vidEvent.SourceHandle);
447+
if (table != null)
448+
{
449+
List<string> choices = NativeMethods.GetEnumPropertyChoices(vidEvent.PropertyHandle);
450+
table.PutStringArray($"PropertyInfo/{vidEvent.Name}/choices", choices);
451+
}
251452
break;
252453
}
253454
case EventKind.SinkSourceChanged:
254-
{
255-
UpdateStreamValues();
256-
break;
257-
}
258455
case EventKind.SinkCreated:
259-
{
260-
break;
261-
}
262456
case EventKind.SinkDestroyed:
263457
{
264-
break;
265-
}
266-
case EventKind.SinkEnabled:
267-
{
268-
break;
269-
}
270-
case EventKind.SinkDisabled:
271-
{
458+
UpdateStreamValues();
272459
break;
273460
}
274461
case EventKind.NetworkInterfacesChanged:
@@ -279,15 +466,64 @@ private CameraServer()
279466
default:
280467
break;
281468
}
282-
}, (EventKind)0x7fff, true);
469+
}, (EventKind)0x4fff, true);
283470

284-
m_tableListener = NtCore.AddEntryListener(PublishName, (uid, key, value, flags) =>
471+
m_tableListener = NtCore.AddEntryListener($"{PublishName}/", (uid, key, value, flags) =>
285472
{
286-
if (!key.StartsWith($"{PublishName}/"))
473+
string relativeKey = key.Substring(PublishName.Length + 1);
474+
475+
int subKeyIndex = relativeKey.IndexOf('/');
476+
if (subKeyIndex == -1) return;
477+
string sourceName = relativeKey.Substring(0, subKeyIndex);
478+
VideoSource source;
479+
if (!m_sources.TryGetValue(sourceName, out source))
480+
{
481+
return;
482+
}
483+
484+
relativeKey = relativeKey.Substring(subKeyIndex + 1);
485+
486+
string propName;
487+
if (relativeKey == "mode")
488+
{
489+
VideoMode mode = VideoModeFromString(value.GetString());
490+
if (mode.PixelFormat == PixelFormat.Unknown || !source.SetVideoMode(mode))
491+
{
492+
NtCore.SetEntryString(key, VideoModeToString(source.GetVideoMode()));
493+
}
494+
return;
495+
}
496+
else if (relativeKey.StartsWith("Property/"))
497+
{
498+
propName = relativeKey.Substring(9);
499+
}
500+
else if (relativeKey.StartsWith("RawProperty/"))
501+
{
502+
propName = relativeKey.Substring(12);
503+
}
504+
else
287505
{
288506
return;
289507
}
290-
string relativeKey = key.Substring(PublishName.Length);
508+
509+
VideoProperty prop = source.GetProperty(propName);
510+
switch (prop.Kind)
511+
{
512+
case PropertyKind.None:
513+
return;
514+
case PropertyKind.Boolean:
515+
prop.Set(value.GetBoolean() ? 1 : 0);
516+
break;
517+
case PropertyKind.Integer:
518+
case PropertyKind.Enum:
519+
prop.Set((int)value.GetDouble());
520+
break;
521+
case PropertyKind.String:
522+
prop.SetString(value.GetString());
523+
break;
524+
default:
525+
throw new ArgumentOutOfRangeException();
526+
}
291527

292528
}, NotifyFlags.NotifyImmediate | NotifyFlags.NotifyUpdate);
293529
}
@@ -423,7 +659,8 @@ public AxisCamera AddAxisCamera(IList<string> hosts)
423659
public AxisCamera AddAxisCamera(string name, string host)
424660
{
425661
AxisCamera camera = new AxisCamera(name, host);
426-
AddCamera(camera);
662+
// Create a passthrough server for USB access
663+
StartAutomaticCapture(camera);
427664
return camera;
428665
}
429666

@@ -436,7 +673,8 @@ public AxisCamera AddAxisCamera(string name, string host)
436673
public AxisCamera AddAxisCamera(string name, IList<string> hosts)
437674
{
438675
AxisCamera camera = new AxisCamera(name, hosts);
439-
AddCamera(camera);
676+
// Create a passthrough server for USB access
677+
StartAutomaticCapture(camera);
440678
return camera;
441679
}
442680

@@ -491,7 +729,7 @@ public CvSink GetVideo(VideoSource camera)
491729
{
492730
throw new VideoException($"expected OpenCV sink, but got {kind}");
493731
}
494-
return (CvSink) sink;
732+
return (CvSink)sink;
495733
}
496734
}
497735

0 commit comments

Comments
 (0)