/// <summary>
/// Create an initial list of stops
/// </summary>
/// <param name="travelTimes">An array of how many seconds it take to go from station to next station, length should be 17</param>
/// <param name="varianceData">An array of how many seconds variance, length should be 17</param>
/// <returns></returns>
private List <Stop> InitiateStops(long[] travelTimes, long[] varianceData)
{
// Create a list of stops
List <Stop> stops = new List <Stop>();
// Create Stop store
Func <EStop, Stop, long, long, Stop> createStop =
(estop, next, travelTime, variance) => new Stop(estop, next, travelTime, variance);
// Create an array in which the roundtrip is defined.
EStop[] roundtrip = new EStop[]
{
// Stops on the trip to Utrecht CS
EStop.P_R_DE_UITHOF,
EStop.WKZ,
EStop.UMC,
EStop.HEIDELBERGLAAN,
EStop.PADUALAAN,
EStop.KROMME_RIJN,
EStop.GALGENWAARD,
EStop.VAARTSCHE_RIJN,
EStop.STATION_CS_CENTRUMZIJDE,
// Stops on the way back to PR
EStop.VAARTSCHE_RIJN,
EStop.GALGENWAARD,
EStop.KROMME_RIJN,
EStop.PADUALAAN,
EStop.HEIDELBERGLAAN,
EStop.UMC,
EStop.WKZ,
EStop.P_R_DE_UITHOF
};
// Create stops
#if DEBUG
DEBUG($"Amount of stops: {roundtrip.Length}");
DEBUG($"Amount of variance data: {varianceData.Length}");
DEBUG($"Amount of travel data: {travelTimes.Length}");
#endif
for (int i = 0; i < roundtrip.Length; i++)
{
stops.Add(createStop(roundtrip[i], null, travelTimes[i], varianceData[i]));
}
// Create next stop connections
for (int i = 0; i < roundtrip.Length - 1; i++)
{
stops[i].NextStop = stops[i + 1];
}
// Connect last stop to first
stops[stops.Count - 1].NextStop = stops[0];
return(stops);
}
private void Window_Loaded(object sender, RoutedEventArgs e)
{
controllerUpdater = new DispatcherTimer()
{
Interval = new TimeSpan(0, 0, 0, 0, 10)
};
controllerUpdater.Tick += Link;
controllerUpdater.Start();
new Thread(() =>
{
XmlRpcUtil.ShowOutputFromXmlRpcPInvoke();
// ROS stuff
ROS.Init(new string[0], "The_UI_" + System.Environment.MachineName.Replace("-", "__"));
nh = new NodeHandle();
Dispatcher.Invoke(new Action(() =>
{
battvolt.startListening(nh);
EStop.startListening(nh);
MotorGraph.startListening(nh);
rosout_control.startListening(nh);
spinningstuff.startListening(nh, "/imu/data");
}));
velPub = nh.advertise <gm.Twist>("/cmd_vel", 1);
multiplexPub = nh.advertise <m.Byte>("/cam_select", 1);
armPub = nh.advertise <am.ArmMovement>("/arm/movement", 1);
ArmON = nh.advertise <m.Bool>("arm/on", 1);
mast_pub = nh.advertise <m.Bool>("raise_camera_mast", 1);
tilt_pub = new Publisher <m.Int32> [4];
for (int i = 0; i < 4; i++)
{
tilt_pub[i] = nh.advertise <m.Int32>("camera" + i + "/tilt", 1);
}
Dispatcher.Invoke(new Action(() =>
{
mainCameras = new TabItem[] { MainCamera1, MainCamera2, MainCamera3, MainCamera4 };
subCameras = new TabItem[] { SubCamera1, SubCamera2, SubCamera3, SubCamera4 };
mainImages = new ROS_ImageWPF.CompressedImageControl[] { camImage0, camImage1, camImage2, camImage3 };
subImages = new ROS_ImageWPF.SlaveImage[] { camImageSlave0, camImageSlave1, camImageSlave2, camImageSlave3 };
for (int i = 0; i < mainCameras.Length; i++)
{
mainImages[i].AddSlave(subImages[i]);
}
subCameras[1].Focus();
adr(false);
// instantiating some global helpers
/*detectors = new DetectionHelper[4];
* for (int i = 0; i < 4; ++i)
* {
* detectors[i] = new DetectionHelper(nh, i, this);
* }*/
}));
#if !INSTANT_DETECTION_DEATH
while (ROS.ok)
{
Dispatcher.Invoke(new Action(() =>
{
for (int i = 0; i < 4; ++i)
{
detectors[i].churnAndBurn();
}
}));
Thread.Sleep(100);
}
#endif
}).Start();
}
