public void UpdateLinkUpBindings()
{
if (Node != null)
{
_CameraEventLabel = Node.GetLabelByName <LinkUpEventLabel>("firefly/computer_vision/camera_event");
_ImuEventLabel = Node.GetLabelByName <LinkUpEventLabel>("firefly/computer_vision/imu_event");
_CameraImuEventLabel = Node.GetLabelByName <LinkUpEventLabel>("firefly/computer_vision/camera_imu_event");
_SlamMapEventLabel = Node.GetLabelByName <LinkUpEventLabel>("firefly/computer_vision/slam_map_event");
_ExposureLabel = Node.GetLabelByName <LinkUpPropertyLabel <Int16> >("firefly/computer_vision/camera_exposure");
_ReplayDataSend = Node.GetLabelByName <LinkUpFunctionLabel>("firefly/computer_vision/replay_data");
_GetRemoteChessboardCornerLabel = Node.GetLabelByName <LinkUpFunctionLabel>("firefly/computer_vision/get_chessboard_corner");
_UpdateSettingsLabel = Node.GetLabelByName <LinkUpFunctionLabel>("firefly/computer_vision/update_settings");
_AccelerometerScaleLabel = Node.GetLabelByName <LinkUpPropertyLabel <Double> >("firefly/computer_vision/acc_scale");
_GyroscopeScaleLabel = Node.GetLabelByName <LinkUpPropertyLabel <Double> >("firefly/computer_vision/gyro_scale");
_TemperatureScaleLabel = Node.GetLabelByName <LinkUpPropertyLabel <Double> >("firefly/computer_vision/temp_scale");
_TemperatureOffsetLabel = Node.GetLabelByName <LinkUpPropertyLabel <Double> >("firefly/computer_vision/temp_offset");
_RecordRemoteLabel = Node.GetLabelByName <LinkUpPropertyLabel <Boolean> >("firefly/computer_vision/record_remote");
_ImuFilterALabel = Node.GetLabelByName <LinkUpPropertyLabel_Binary>("firefly/computer_vision/imu_filter_a");
_ImuFilterBLabel = Node.GetLabelByName <LinkUpPropertyLabel_Binary>("firefly/computer_vision/imu_filter_b");
_ImuDerivedEventLabel = Node.GetLabelByName <LinkUpEventLabel>("firefly/computer_vision/imu_derived_event");
_SlamStatusEventLabel = Node.GetLabelByName <LinkUpEventLabel>("firefly/computer_vision/slam_status_event");
_SlamChangeStatusLabel = Node.GetLabelByName <LinkUpFunctionLabel>("firefly/computer_vision/slam_change_status");
_SlamReproducibleExecutionLabel = Node.GetLabelByName <LinkUpPropertyLabel <Boolean> >("firefly/computer_vision/slam_reproducible_execution");
_TCamImuLabel = Node.GetLabelByName <LinkUpPropertyLabel_Binary>("firefly/computer_vision/calibration_T_cam_imu");
_RAccGyroLabel = Node.GetLabelByName <LinkUpPropertyLabel_Binary>("firefly/computer_vision/calibration_R_acc_gyro");
_MInvGyroLabel = Node.GetLabelByName <LinkUpPropertyLabel_Binary>("firefly/computer_vision/calibration_M_inv_gyro");
_MInvAccLabel = Node.GetLabelByName <LinkUpPropertyLabel_Binary>("firefly/computer_vision/calibration_M_inv_acc");
_GyroscopeNoiseLabel = Node.GetLabelByName <LinkUpPropertyLabel <Double> >("firefly/computer_vision/calibration_gyro_noise");
_GyroscopeWalkLabel = Node.GetLabelByName <LinkUpPropertyLabel <Double> >("firefly/computer_vision/calibration_gyro_walk");
_AccelerometerNoiseLabel = Node.GetLabelByName <LinkUpPropertyLabel <Double> >("firefly/computer_vision/calibration_acc_noise");
_AccelerometerWalkLabel = Node.GetLabelByName <LinkUpPropertyLabel <Double> >("firefly/computer_vision/calibration_acc_walk");
_EnableVisualInertialLabel = Node.GetLabelByName <LinkUpPropertyLabel <Boolean> >("firefly/computer_vision/slam_enable_visual_inertial");
_ImuDerivedEventLabel.Fired += _ImuFilterEvent_Fired;
_SlamStatusEventLabel.Fired += _SlamStatusEventLabel_Fired;
_CameraEventLabel.Fired += _CameraEventLabel_Fired;
_ImuEventLabel.Fired += _CameraEventLabel_Fired;
_CameraImuEventLabel.Fired += _CameraEventLabel_Fired;
_SlamMapEventLabel.Fired += _SlamMapEventLabel_Fired;
Update();
}
}
private static void Program_Fired(LinkUpEventLabel label, byte[] data)
{
if (count == 0)
{
stopWatch = new Stopwatch();
stopWatch.Start();
}
count++;
bytes += data.Length;
if (count % 100 == 0)
{
Console.WriteLine("{0:0.0} events/s\t{1:0.0} KB/s\t{2:0.0} MBit/s", ((double)count) / stopWatch.ElapsedMilliseconds * 1000, ((double)bytes) / stopWatch.ElapsedMilliseconds * 1000 / 1024, ((double)bytes) / stopWatch.ElapsedMilliseconds * 1000 / 1024 / 1024 * 8);
stopWatch.Restart();
count = 0;
bytes = 0;
func.AsyncCall(new byte[2]);
}
//onsole.WriteLine("- EVENT ({0}): {1}", label.Name, data.Length/*string.Join(" ", data.Select(b => string.Format("{0:X2} ", b)))*/);
}
private void _SlamStatusEventLabel_Fired(LinkUpEventLabel label, byte[] data)
{
List <Tuple <IProxyEventSubscriber, ProxyEventType> > subscriber = null;
lock (_Subscriptions)
{
subscriber = _Subscriptions.Where(c => c.Item2 == ProxyEventType.SlamStatusEvent).ToList();
}
if (label == _SlamStatusEventLabel && subscriber != null && subscriber.Count() > 0)
{
SlamStatusEventData eventData = SlamStatusEventData.Parse(data);
foreach (Tuple <IProxyEventSubscriber, ProxyEventType> t in subscriber)
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
eventData
});
}
}
}
private static void Main(string[] args)
{
using (LinkUpTcpClientConnector connector = new LinkUpTcpClientConnector(IPAddress.Parse("127.0.0.1"), 3000))
//using (LinkUpTcpClientConnector connector = new LinkUpTcpClientConnector(IPAddress.Parse("127.0.0.1"), 3000))
{
connector.ReveivedPacket += ClientToServer_ReveivedPacket;
connector.SentPacket += ClientToServer_SentPacket;
connector.MetricUpdate += Connector_MetricUpdate;
connector.ConnectivityChanged += Connector_ConnectivityChanged;
node = new LinkUpNode();
node.Name = "leaf";
node.AddSubNode(connector);
LinkUpEventLabel eventLabel = node.GetLabelByName <LinkUpEventLabel>("leaf/test/label_event");
func = node.GetLabelByName <LinkUpFunctionLabel>("leaf/test/label_function");
func.Return += Func_Return;
eventLabel.Subscribe();
eventLabel.Fired += Program_Fired;
Console.Read();
connector.Dispose();
}
}
private void _CameraEventLabel_Fired(LinkUpEventLabel label, byte[] data)
{
List <Tuple <IProxyEventSubscriber, ProxyEventType> > subscriberImu = null;
List <Tuple <IProxyEventSubscriber, ProxyEventType> > subscriberCamImu = null;
List <Tuple <IProxyEventSubscriber, ProxyEventType> > subscriberCam = null;
lock (_Subscriptions)
{
subscriberCam = _Subscriptions.Where(c => c.Item2 == ProxyEventType.CameraEvent).ToList();
subscriberImu = _Subscriptions.Where(c => c.Item2 == ProxyEventType.ImuEvent).ToList();
subscriberCamImu = _Subscriptions.Where(c => c.Item2 == ProxyEventType.CameraImuEvent).ToList();
}
if (label == _CameraEventLabel && subscriberCam != null && subscriberCam.Count() > 0)
{
CameraEventData eventData = CameraEventData.Parse(data, 0, true);
foreach (Tuple <IProxyEventSubscriber, ProxyEventType> t in subscriberCam)
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
eventData
});
}
}
else if (label == _ImuEventLabel && subscriberImu != null && subscriberImu.Count() > 0)
{
ImuEventData eventData = ImuEventData.Parse(data, 0, _SettingContainer.Settings.ImuSettings.GyroscopeScale, _SettingContainer.Settings.ImuSettings.AccelerometerScale, _SettingContainer.Settings.ImuSettings.TemperatureScale, _SettingContainer.Settings.ImuSettings.TemperatureOffset);
foreach (Tuple <IProxyEventSubscriber, ProxyEventType> t in subscriberImu)
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
eventData
});
}
}
else if (label == _CameraImuEventLabel)
{
ImuEventData imuEventData = ImuEventData.Parse(data, 0, _SettingContainer.Settings.ImuSettings.GyroscopeScale, _SettingContainer.Settings.ImuSettings.AccelerometerScale, _SettingContainer.Settings.ImuSettings.TemperatureScale, _SettingContainer.Settings.ImuSettings.TemperatureOffset);
CameraEventData cameraEventData = null;
if (imuEventData.HasCameraImage)
{
cameraEventData = CameraEventData.Parse(data, 23, true);
}
if (subscriberCamImu != null && subscriberCamImu.Count() > 0)
{
foreach (Tuple <IProxyEventSubscriber, ProxyEventType> t in subscriberCamImu)
{
if (cameraEventData != null)
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
cameraEventData, imuEventData
});
}
else
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
imuEventData
});
}
}
}
if (subscriberCam != null && subscriberCam.Count() > 0)
{
foreach (Tuple <IProxyEventSubscriber, ProxyEventType> t in subscriberCam)
{
if (cameraEventData != null)
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
cameraEventData
});
}
}
}
if (subscriberImu != null && subscriberImu.Count() > 0)
{
foreach (Tuple <IProxyEventSubscriber, ProxyEventType> t in subscriberImu)
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
imuEventData
});
}
}
lastTimestamp = imuEventData.TimeNanoSeconds;
}
}
