public void MessageArrived(string channelName, object message)
{
if (channelName == "PositionChannel")
{
this.vehicleState = (VehicleState)message;
}
else if (channelName == "ArbiterInformationChannel")
{
this.ArbiterInformation = (ArbiterInformation)message;
}
else if (channelName == "CarMode")
{
this.CarMode = (CarMode)message;
}
else if (channelName == "ObservedVehicleChannel")
{
this.observedVehicles = (ObservedVehicles)message;
}
else if (channelName == "ObservedObstacleChannel")
{
this.observedObstacles = (ObservedObstacles)message;
}
else if (channelName == "FakeVehicleChannel")
{
this.fakeVehicles = (ObservedVehicles)message;
}
else
{
throw new ArgumentException("Unknown Channel", channelName);
}
}
public static void UpdatePresentation(ArbiterInformation arbiterInformation)
{
using (var repo = new CRUDRepository <ArbiterInformation>())
{
if (arbiterInformation != null)
{
var previous = repo.GetQuery <ArbiterInformation>(c => c.Id == arbiterInformation.Id).FirstOrDefault();
if (previous != null)
{
previous.Presentation = arbiterInformation.Presentation;
repo.Update(previous);
}
}
}
}
public ContentResult UpdateArbiterPresentation(ArbiterInformation arbiterInformation)
{
try
{
BLLArbiter.UpdatePresentation(arbiterInformation);
return(new ContentResult()
{
Content = "OK"
});
}
catch (Exception e)
{
return(new ContentResult()
{
Content = "KO"
});
}
}
public void UpdateInformation(ArbiterInformation arbiterInformation)
{
if (arbiterInformation != null)
{
//Stopwatch stopwatch3 = new Stopwatch();
//stopwatch3.Reset();
//stopwatch3.Start();
ArbiterOutput.WriteToLog(arbiterInformation.LogString());
//stopwatch3.Stop();
//Console.WriteLine("SW 3: " + stopwatch3.ElapsedMilliseconds.ToString());
//stopwatch3.Reset();
//stopwatch3.Start();
this.currentInformation = arbiterInformation;
//stopwatch3.Stop();
//Console.WriteLine("SW 4: " + stopwatch3.ElapsedMilliseconds.ToString());
}
}
public VehicleSeparationDisplay()
{
InitializeComponent();
this.Information = null;
this.comms = new Communicator(this);
// set our style
base.SetStyle(ControlStyles.UserPaint, true);
base.SetStyle(ControlStyles.AllPaintingInWmPaint, true);
base.SetStyle(ControlStyles.Opaque, true);
base.SetStyle(ControlStyles.OptimizedDoubleBuffer, true);
base.SetStyle(ControlStyles.ResizeRedraw, true);
base.SetStyle(ControlStyles.Selectable, true);
Thread t = new Thread(ForceRedraw);
t.IsBackground = true;
t.Start();
client = new PoseClient(IPAddress.Parse("239.132.1.33"), 4839);
client.PoseAbsReceived += new EventHandler <PoseAbsReceivedEventArgs>(client_PoseAbsReceived);
client.Start();
}
public void UpdateInformation()
{
// generate the new items
ArbiterInformationDisplay aid = RemoraCommon.aiInformation;
if (aid != null && aid.information != null)
{
// check items
if (aid.information.LAConsistent != "" || aid.information.LAInitial != "" || aid.information.LAPosteriorProbInitial != "" ||
aid.information.LAPosteriorProbTarget != "" || aid.information.LAProbabilityCorrect != "" || aid.information.LASceneLikelyLane != "" ||
aid.information.LATarget != "")
{
// clear out the list view
this.laneAgentData.Items.Clear();
ArbiterInformation ai = aid.information;
ListViewItem lvf = new ListViewItem(new string[] { "Internal State", "Ai Filtered", ai.LAProbabilityCorrect });
ListViewItem lvi = new ListViewItem(new string[] { ai.LAInitial, "Pose Initial", ai.LAPosteriorProbInitial });
ListViewItem lvt = new ListViewItem(new string[] { ai.LATarget, "Pose Target", ai.LAPosteriorProbTarget });
ListViewItem lvc = new ListViewItem(new string[] { "", "Pose Consistency", ai.LAConsistent });
ListViewItem lvs = new ListViewItem(new string[] { ai.LASceneLikelyLane, "Pose Likely", "" });
this.laneAgentData.Items.Add(lvf);
this.laneAgentData.Items.Add(lvi);
this.laneAgentData.Items.Add(lvt);
this.laneAgentData.Items.Add(lvc);
this.laneAgentData.Items.Add(lvs);
}
else
{
if (this.laneAgentData.Items.Count > 0)
{
this.laneAgentData.Items.Clear();
}
}
}
}
public void Render(System.Drawing.Graphics g, WorldTransform t)
{
ArbiterInformation tmpInfo = information;
if (tmpInfo != null)
{
lock (tmpInfo)
{
if (tmpInfo.Route1 != null && tmpInfo.Route1.RoutePlan != null && DrawingUtility.DrawNavigationBestRoute)
{
Color c = DrawingUtility.ColorNavigationBest;
for (int j = 0; j < tmpInfo.Route1.RoutePlan.Count; j++)
{
if (j == 0 && RemoraCommon.Communicator.GetVehicleState() != null)
{
DrawingUtility.DrawColoredControlLine(c, System.Drawing.Drawing2D.DashStyle.Solid, RemoraCommon.Communicator.GetVehicleState().Position, tmpInfo.Route1.RoutePlan[j], g, t);
}
else if (RemoraCommon.Communicator.GetVehicleState() != null)
{
DrawingUtility.DrawColoredControlLine(c, System.Drawing.Drawing2D.DashStyle.Solid, tmpInfo.Route1.RoutePlan[j - 1], tmpInfo.Route1.RoutePlan[j], g, t);
}
}
}
foreach (ArbiterInformationDisplayObject aido in tmpInfo.DisplayObjects)
{
if (aido.Type == ArbiterInformationDisplayObjectType.uTurnPolygon)
{
Polygon p = (Polygon)aido.DisplayObject;
if (p.Count > 1)
{
for (int i = 1; i < p.Count; i++)
{
DrawingUtility.DrawColoredControlLine(Color.Orange, System.Drawing.Drawing2D.DashStyle.Solid, p[i - 1], p[i], g, t);
}
DrawingUtility.DrawColoredControlLine(Color.Orange, System.Drawing.Drawing2D.DashStyle.Solid, p[0], p[p.Count - 1], g, t);
}
}
else if (aido.Type == ArbiterInformationDisplayObjectType.leftBound)
{
LineList ll = (LineList)aido.DisplayObject;
if (ll.Count > 1)
{
for (int i = 1; i < ll.Count; i++)
{
DrawingUtility.DrawColoredControlLine(Color.Blue, System.Drawing.Drawing2D.DashStyle.Solid, ll[i - 1], ll[i], g, t);
}
}
}
else if (aido.Type == ArbiterInformationDisplayObjectType.rightBound)
{
LineList ll = (LineList)aido.DisplayObject;
if (ll.Count > 1)
{
for (int i = 1; i < ll.Count; i++)
{
DrawingUtility.DrawColoredControlLine(Color.Red, System.Drawing.Drawing2D.DashStyle.Solid, ll[i - 1], ll[i], g, t);
}
}
}
}
}
}
}
/// <summary>
/// Updates visible information
/// </summary>
public void UpdateInformation()
{
if (RemoraCommon.aiInformation != null)
{
// get ai info
ArbiterInformation ai = RemoraCommon.aiInformation.information;
if (ai != null)
{
this.state.Text = ai.CurrentState;
this.stateInfo.Text = ai.CurrentStateInfo;
this.blockageDisplayLabel.Text = ai.Blockage;
this.fqmWaypoint.Text = ai.FQMWaypoint;
this.fqmSpeed.Text = ai.FQMSpeed;
this.fqmDistance.Text = ai.FQMDistance;
this.fqmStopType.Text = ai.FQMStopType;
this.fqmNextState.Text = ai.FQMState;
this.fqmNextStateInfo.Text = ai.FQMStateInfo;
this.fqmBehavior.Text = ai.FQMBehavior;
this.fqmBehaviorInfo.Text = ai.FQMBehaviorInfo;
this.fqmSpeedCommand.Text = ai.FQMSpeedCommand;
this.segmentSpeeds.Text = ai.FQMSegmentSpeedLimit;
this.nextState.Text = ai.NextState;
this.nextStateInfo.Text = ai.NextStateInfo;
this.behavior.Text = ai.NextBehavior;
this.behaviorInfo.Text = ai.NextBehaviorInfo;
this.speedCommand.Text = ai.NextSpeedCommand;
this.turnSignalsLabel.Text = ai.NextBehaviorTurnSignals;
this.timestampLabel.Text = ai.NextBehaviorTimestamp;
this.route1Waypoint.Text = ai.Route1Wp;
this.route1Time.Text = ai.Route1Time;
this.route2Waypoint.Text = ai.Route2Wp;
this.route2Time.Text = ai.Route2Time;
this.checkpoint.Text = ai.RouteCheckpoint;
this.goalsRemainingText.Text = ai.GoalsRemaining;
this.checkpointId.Text = ai.RouteCheckpointId;
this.fvSpeed.Text = ai.FVTSpeed;
this.fvSpeedCommand.Text = ai.FVTSpeedCommand;
this.fvNextStateInfo.Text = ai.FVTStateInfo;
this.fvNextState.Text = ai.FVTState;
this.fvNextBehavior.Text = ai.FVTBehavior;
this.fvDistance.Text = ai.FVTDistance;
this.FVTxSeparationLabel.Text = ai.FVTXSeparation;
this.FVTIgnoredLabel.Text = "";
if (ai.FVTIgnorable != null && ai.FVTIgnorable.Length > 0)
{
for (int i = 0; i < ai.FVTIgnorable.Length; i++)
{
this.FVTIgnoredLabel.Text += ai.FVTIgnorable[i];
this.FVTIgnoredLabel.Text += i == ai.FVTIgnorable.Length - 1 ? "" : ", ";
}
}
VehicleState vs = RemoraCommon.Communicator.GetVehicleState();
if (vs != null)
{
this.posteriorPoseEast.Text = vs.Position.X.ToString("F3");
this.posteriorPoseNorth.Text = vs.Position.Y.ToString("F3");
}
if (RemoraCommon.Communicator.GetVehicleSpeed().HasValue)
{
this.posteriorPoseSpeed.Text = RemoraCommon.Communicator.GetVehicleSpeed().Value.ToString("F3");
}
this.CarModeLabel.Text = RemoraCommon.Communicator.GetCarMode().ToString();
this.Invalidate();
}
}
}
/// <summary>
/// Monitors output of the vehicle
/// </summary>
private void OutputThread()
{
// create a new stopwatch
Stopwatch stopwatch = new Stopwatch();
// while supposed to listen for data
while (runOutput)
{
try
{
// start teh timer
stopwatch.Reset();
stopwatch.Start();
// reset the display, adding grid and rndf and pose log
List <IDisplayObject> newDisplayObjects = new List <IDisplayObject>();
newDisplayObjects.Add(new DisplayGrid());
newDisplayObjects.Add(new RndfDisplay(this.roadDisplay1.rndf));
newDisplayObjects.Add(this.poseLog);
// vehicle information fields
string positionX = "";
string positionY = "";
string degreesHeading = "";
string speed = "";
string carMode = "";
// check for fake vehicles
if (this.communicator.channelListener.FakeVehicles != null && this.communicator.channelListener.FakeVehicles.Value.Vehicles != null &&
this.communicator.channelListener.FakeVehicles.Value.Vehicles.Length > 0)
{
// add fakes
foreach (ObservedVehicle ov in this.communicator.channelListener.FakeVehicles.Value.Vehicles)
{
newDisplayObjects.Add(new ObservedVehicleDisplay(ov, Color.Orange));
}
}
// make sure the vehicle exists
if (this.communicator.channelListener.Vehicle != null)
{
// draw static obstacles
if (this.communicator.channelListener.ObservedObstacles != null)
{
newDisplayObjects.Add(new StaticObstaclesDisplay(this.communicator.channelListener.ObservedObstacles.Value,
this.communicator.channelListener.Vehicle));
}
if (this.communicator.channelListener.ObservedVehicles != null)
{
foreach (ObservedVehicle ov in this.communicator.channelListener.ObservedVehicles.Value.Vehicles)
{
newDisplayObjects.Add(new ObservedVehicleDisplay(ov));
}
}
// vehicle information fields
positionX = this.communicator.channelListener.Vehicle.xyPosition.X.ToString();
positionY = this.communicator.channelListener.Vehicle.xyPosition.Y.ToString();
degreesHeading = this.communicator.channelListener.Vehicle.heading.ToDegrees().ToString();
speed = this.communicator.channelListener.Vehicle.speed.ToString();
// car mode
carMode = this.communicator.CarMode.ToString();
// add to log
if (DrawingUtility.LogPose)
{
this.poseLog.Update(this.communicator.channelListener.Vehicle.xyPosition);
}
// add the car to the display objects
newDisplayObjects.Add(new CarDisplay(this.communicator.channelListener.Vehicle.xyPosition, this.communicator.channelListener.Vehicle.heading, true));
// vehicle tracking
if (Globals.TrackVehicle)
{
// Get the offset.
Point point = new Point(this.roadDisplay1.ClientRectangle.Width / 2, this.roadDisplay1.ClientRectangle.Height / 2);
// get screen po of vehicle
PointF screenCarPos = this.roadDisplay1.transform.GetScreenPoint(this.communicator.channelListener.Vehicle.xyPosition);
// Calculate change in Position
double deltaX = ((double)screenCarPos.X) - point.X;
double deltaY = ((double)screenCarPos.Y) - point.Y;
// Update the world
Coordinates tempCenter = this.roadDisplay1.transform.CenterPoint;
tempCenter.X += deltaX / this.roadDisplay1.transform.Scale;
tempCenter.Y -= deltaY / this.roadDisplay1.transform.Scale;
this.roadDisplay1.transform.CenterPoint = tempCenter;
}
}
// arbiter information fields
string routeTime = "";
string arbiterState = "";
string currentGoal = "";
string goalsLeft = "";
string routeDistance = "";
string arbiterCarMode = "";
// make sure the arbiter state update exists
if (this.communicator.channelListener.ArbiterInformation != null)
{
// get the arbiter information class
ArbiterInformation arbiterInfo = this.communicator.channelListener.ArbiterInformation;
// route
if (arbiterInfo.FullRoute != null)
{
newDisplayObjects.Add(new RouteDisplay(this.roadDisplay1.rndf, this.communicator.channelListener.ArbiterInformation.FullRoute));
}
// route info
routeTime = arbiterInfo.RouteTime.ToString();
// goal
if (arbiterInfo.CurrentGoal != null && this.roadDisplay1.rndf != null)
{
newDisplayObjects.Add(new GoalsDisplay(this.roadDisplay1.rndf, arbiterInfo.CurrentGoal, null));
}
// goals left
if (arbiterInfo.Goals != null)
{
goalsLeft = arbiterInfo.Goals.Count.ToString();
}
// route distance
if (arbiterInfo.FullRoute != null)
{
double distance = 0;
for (int i = 0; i < arbiterInfo.FullRoute.RouteNodes.Count - 1; i++)
{
distance += this.roadDisplay1.rndf.Waypoints[arbiterInfo.FullRoute.RouteNodes[i]].Position.DistanceTo(
this.roadDisplay1.rndf.Waypoints[arbiterInfo.FullRoute.RouteNodes[i + 1]].Position);
}
routeDistance = distance.ToString();
}
// arbiter state
arbiterState = this.communicator.channelListener.ArbiterInformation.CurrentArbiterState;
// uturn polygon
if (arbiterInfo.Behavior != null && arbiterInfo.Behavior is UTurnBehavior)
{
newDisplayObjects.Add(new PolygonDisplay(((UTurnBehavior)arbiterInfo.Behavior).BoundingPolygon));
}
// current goal
if (this.communicator.channelListener.ArbiterInformation.CurrentGoal != null)
{
currentGoal = this.communicator.channelListener.ArbiterInformation.CurrentGoal.LaneID.ToString()
+ "." + this.communicator.channelListener.ArbiterInformation.CurrentGoal.WaypointNumber.ToString();
}
// car mode
arbiterCarMode = arbiterInfo.InternalCarMode.ToString();
}
// set new display objects
this.roadDisplay1.displayObjects = newDisplayObjects;
// redraw
this.roadDisplay1.Invalidate();
this.OnResize(new EventArgs());
// set display of vehicle information
this.BeginInvoke(new MethodInvoker(delegate()
{
// Position
if (positionX.Length >= 12 && positionY.Length >= 12)
{
this.GeneralTabPosition.Text = positionX.Substring(0, 12) + ", " + positionY.Substring(0, 12);
}
else
{
this.GeneralTabPosition.Text = positionX + ", " + positionY;
}
// heading
if (degreesHeading.Length >= 8)
{
this.GeneralTabHeading.Text = degreesHeading.Substring(0, 8) + " degrees";
}
else
{
this.GeneralTabHeading.Text = degreesHeading + " degrees";
}
// speed
if (speed.Length >= 8)
{
this.GeneralTabSpeed.Text = speed.Substring(0, 8) + " m/s";
}
else
{
this.GeneralTabSpeed.Text = speed + " m/s";
}
/*// lane estimates
* if (this.communicator.channelListener.Vehicle.laneEstimate != null)
* {
* this.GeneralTabInitialLane.Text = this.communicator.channelListener.Vehicle.laneEstimate.InitialLane.ToString();
* this.GeneralTabFinalLane.Text = this.communicator.channelListener.Vehicle.laneEstimate.TargetLane.ToString();
* this.GeneralTabLaneEstimateConfidence.Text = this.communicator.channelListener.Vehicle.laneEstimate.Confidence.ToString();
* }*/
// set route time
this.GeneralTabRouteTimeText.Text = routeTime;
// current goal
this.GeneralTabCurrentGoalText.Text = currentGoal;
// arbiter state
this.ArbiterStateTextGeneralTab.Text = arbiterState;
// route distance
this.GeneralTabArbiterRouteDistanceText.Text = routeDistance;
// goals left
this.GeneralTabGoalsLeftLabelText.Text = goalsLeft;
// internal car mode
this.GeneralTabArbiterCarModeText.Text = arbiterCarMode;
// set external car mode
this.CarModeLabelTextGeneralTab.Text = carMode;
}));
// stop the stopwatch
stopwatch.Stop();
// check if we have reached the elapsed time yet
if (stopwatch.ElapsedMilliseconds < 50)
{
// sleep so we can get 50ms cycles
Thread.Sleep(50 - (int)stopwatch.ElapsedMilliseconds);
}
}
catch (Exception ex)
{
Debug.WriteLine(ex.ToString() + "\n\n" + ex.StackTrace);
}
}
}
