/// <summary>
/// Check if a side sick blockng obstacle is detected
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <returns></returns>
private bool SideSickObstacleDetected(ArbiterLane lane, VehicleState state)
{
try
{
// get distance from vehicle to lane opp side
Coordinates vec = state.Front - state.Position;
vec = this.VehicleSide == SideObstacleSide.Driver ? vec.Rotate90() : vec.RotateM90();
SideObstacles sobs = CoreCommon.Communications.GetSideObstacles(this.VehicleSide);
if (sobs != null && sobs.obstacles != null)
{
foreach (SideObstacle so in sobs.obstacles)
{
Coordinates cVec = state.Position + vec.Normalize(so.distance);
if (so.height > 0.7 && lane.LanePolygon.IsInside(cVec))
{
return(true);
}
}
}
}
catch (Exception ex)
{
ArbiterOutput.Output("opposing side sick obstacle exception: " + ex.ToString());
}
return(false);
}
public void OnSideObstaclesReceived(SideObstacles obstacles)
{
if (obstacles.side == SideObstacleSide.Passenger)
{
currentRightSideObstacles = obstacles;
}
else
{
currentLeftSideObstacles = obstacles;
}
}
/// <summary>
/// Called when message sent to us
/// </summary>
/// <param name="channelName"></param>
/// <param name="message"></param>
public void MessageArrived(string channelName, object message)
{
if (channelName == "ArbiterSceneEstimatorPositionChannel" + RemoraCommon.Communicator.RemotingSuffix &&
message is VehicleState)
{
// cast and set
vehicleState = (VehicleState)message;
}
else if (channelName == "ObservedObstacleChannel" + RemoraCommon.Communicator.RemotingSuffix &&
message is SceneEstimatorUntrackedClusterCollection)
{
// cast and set
observedObstacles = (SceneEstimatorUntrackedClusterCollection)message;
}
else if (channelName == "ObservedVehicleChannel" + RemoraCommon.Communicator.RemotingSuffix &&
message is SceneEstimatorTrackedClusterCollection)
{
// cast and set
observedVehicles = (SceneEstimatorTrackedClusterCollection)message;
}
else if (channelName == "VehicleSpeedChannel" + RemoraCommon.Communicator.RemotingSuffix &&
message is double)
{
// cast and set
vehicleSpeed = (double)message;
}
else if (channelName == "ArbiterOutputChannel" + RemoraCommon.Communicator.RemotingSuffix &&
message is string)
{
// output
RemoraOutput.WriteLine((string)message, OutputType.Arbiter);
}
else if (channelName == "ArbiterInformationChannel" + RemoraCommon.Communicator.RemotingSuffix &&
message is ArbiterInformation)
{
// set info
RemoraCommon.aiInformation.information = (ArbiterInformation)message;
}
else if (channelName == "SideObstacleChannel" + RemoraCommon.Communicator.RemotingSuffix &&
message is SideObstacles)
{
SideObstacles sideSickObstacles = (SideObstacles)message;
if (sideSickObstacles.side == SideObstacleSide.Driver)
{
this.sideSickObstaclesDriver = sideSickObstacles;
}
else
{
this.sideSickObstaclesPass = sideSickObstacles;
}
}
}
public static Object Deserialize(Stream stream, string channelName)
{
BinaryReader br = new BinaryReader(stream);
SideObstacleMsgID msgtype = (SideObstacleMsgID)br.ReadInt32();
switch (msgtype)
{
case SideObstacleMsgID.Info:
Console.WriteLine("SO Info:");
break;
case SideObstacleMsgID.Bad:
Console.WriteLine("SO BAD:");
break;
case SideObstacleMsgID.ScanMsg:
{
SideObstacles sideObstacles = new SideObstacles();
sideObstacles.side = (SideObstacleSide)br.ReadInt32();
sideObstacles.timestamp = br.ReadDouble();
int numobstacles = br.ReadInt32();
sideObstacles.obstacles = new List <SideObstacle>();
for (int i = 0; i < numobstacles; i++)
{
SideObstacle obstacle = new SideObstacle();
obstacle.distance = br.ReadSingle();
int points = br.ReadInt32();
obstacle.height = br.ReadSingle();
sideObstacles.obstacles.Add(obstacle);
}
for (int i = numobstacles; i < 10; i++)
{
br.ReadSingle();
br.ReadInt32();
br.ReadSingle();
}
if (br.BaseStream.Position != br.BaseStream.Length)
{
Console.WriteLine("WARNING: Incomplete read of side sick msg.");
}
return(sideObstacles);
}
default:
throw new InvalidDataException("Invalid SideObstaclesSerializer Message Received: " + msgtype);
}
return(null);
}
/// <summary>
/// Called when message sent to us
/// </summary>
/// <param name="channelName"></param>
/// <param name="message"></param>
public void MessageArrived(string channelName, object message)
{
try
{
if (channelName == "ArbiterSceneEstimatorPositionChannel" + this.remotingSuffix &&
message is VehicleState)
{
// cast and set
vehicleState = (VehicleState)message;
}
else if (channelName == "ObservedObstacleChannel" + this.remotingSuffix &&
message is SceneEstimatorUntrackedClusterCollection)
{
// cast and set
observedObstacles = (SceneEstimatorUntrackedClusterCollection)message;
}
else if (channelName == "ObservedVehicleChannel" + this.remotingSuffix &&
message is SceneEstimatorTrackedClusterCollection)
{
// check if not ignoring vehicles
if (!Arbiter.Core.ArbiterSettings.Default.IgnoreVehicles)
{
// cast and set
observedVehicles = (SceneEstimatorTrackedClusterCollection)message;
}
}
else if (channelName == "VehicleSpeedChannel" + this.remotingSuffix &&
message is double)
{
// cast and set
vehicleSpeed = (double)message;
}
else if (channelName == "SideObstacleChannel" + this.remotingSuffix &&
message is SideObstacles)
{
SideObstacles sideSickObstacles = (SideObstacles)message;
if (sideSickObstacles.side == SideObstacleSide.Driver)
{
this.sideSickObstaclesDriver = sideSickObstacles;
}
else
{
this.sideSickObstaclesPass = sideSickObstacles;
}
}
}
catch (Exception ex)
{
ArbiterOutput.Output("Error receiving message: " + ex.ToString());
}
}
private Obstacle GetSideObstacle(SideObstacles sideObstacles)
{
if (sideObstacles == null)
{
return(null);
}
double minDist = 100;
Obstacle minObstacle = null;
foreach (SideObstacle obs in sideObstacles.obstacles)
{
if (obs.distance > 0.5 && obs.distance < minDist)
{
minDist = obs.distance;
// create a polygon the specified distance away and 1 m in front of the rear axle and 0.5 m behind the front axle
Polygon poly = new Polygon();
if (sideObstacles.side == SideObstacleSide.Driver)
{
poly.Add(new Coordinates(1, obs.distance + TahoeParams.T / 2.0));
poly.Add(new Coordinates(TahoeParams.FL - 0.5, obs.distance + TahoeParams.T / 2.0));
poly.Add(new Coordinates(TahoeParams.FL - 0.5, obs.distance + TahoeParams.T / 2.0 + 1));
poly.Add(new Coordinates(1, obs.distance + TahoeParams.T / 2.0 + 1));
}
else
{
poly.Add(new Coordinates(1, -(obs.distance + TahoeParams.T / 2.0)));
poly.Add(new Coordinates(TahoeParams.FL - 0.5, -(obs.distance + TahoeParams.T / 2.0)));
poly.Add(new Coordinates(TahoeParams.FL - 0.5, -(obs.distance + TahoeParams.T / 2.0 + 1)));
poly.Add(new Coordinates(1, -(obs.distance + TahoeParams.T / 2.0 + 1)));
}
Obstacle finalObs = new Obstacle();
finalObs.age = 1;
finalObs.obstacleClass = ObstacleClass.StaticLarge;
finalObs.obstaclePolygon = poly;
finalObs.minSpacing = min_spacing[(int)finalObs.obstacleClass];
finalObs.desSpacing = des_spacing[(int)finalObs.obstacleClass];
finalObs.cspacePolygon = Polygon.ConvexMinkowskiConvolution(conv_polygon[(int)finalObs.obstacleClass], finalObs.AvoidancePolygon);
minObstacle = finalObs;
}
}
return(minObstacle);
}
private SideObstacle GetMinSideObstacle(SideObstacles sideObstacles)
{
if (sideObstacles == null)
{
return(null);
}
double minDist = 100;
SideObstacle minObstacle = null;
foreach (SideObstacle obs in sideObstacles.obstacles)
{
if (obs.distance > 0.5 && obs.distance < minDist)
{
minDist = obs.distance;
minObstacle = obs;
}
}
return(minObstacle);
}
/// <summary>
/// What happens when we paint
/// </summary>
/// <param name="e"></param>
protected override void OnPaint(PaintEventArgs e)
{
// clear the background
e.Graphics.Clear(BackColor);
// center on tracked vehicle if exists
if (this.tracked != null)
{
// Get the offset.
Point point = new Point(this.ClientRectangle.Width / 2, this.ClientRectangle.Height / 2);
// get screen po of vehicle
PointF screenCarPos = this.transform.GetScreenPoint(this.tracked.Position);
// Calculate change in Position
double deltaX = ((double)screenCarPos.X) - point.X;
double deltaY = ((double)screenCarPos.Y) - point.Y;
// Update the world
Coordinates tempCenter = WorldTransform.CenterPoint;
tempCenter.X += deltaX / WorldTransform.Scale;
tempCenter.Y -= deltaY / WorldTransform.Scale;
WorldTransform.CenterPoint = tempCenter;
}
// save the graphics state
GraphicsState gs = e.Graphics.Save();
// set the drawing modes
e.Graphics.SmoothingMode = SmoothingMode.AntiAlias; //.HighSpeed;//.HighQuality;
e.Graphics.CompositingQuality = CompositingQuality.HighSpeed; //.HighQuality;
e.Graphics.InterpolationMode = InterpolationMode.Bilinear; //.Bicubic;
e.Graphics.TextRenderingHint = System.Drawing.Text.TextRenderingHint.ClearTypeGridFit;
// set the transform
e.Graphics.Transform = transform.GetTransform();
// paint the display objects
foreach (IDisplayObject obj in displayObjects)
{
// check if we should display the object
if (obj.ShouldDraw())
{
// render each object
obj.Render(e.Graphics, transform);
}
}
// render ai vehicle
if (!this.DesignMode && this.aiVehicle != null && this.aiVehicle.State != null && this.aiVehicle.ShouldDraw())
{
this.aiVehicle.Render(e.Graphics, transform);
}
// render ai information
if (!this.DesignMode && RemoraCommon.aiInformation.ShouldDraw())
{
RemoraCommon.aiInformation.Render(e.Graphics, transform);
}
// render current tool
if (!this.DesignMode && this.CurrentEditorTool != null && this.CurrentEditorTool.ShouldDraw())
{
this.CurrentEditorTool.Render(e.Graphics, transform);
}
// render current tool
if (!this.DesignMode && this.SecondaryEditorTool != null && this.SecondaryEditorTool.ShouldDraw())
{
this.SecondaryEditorTool.Render(e.Graphics, transform);
}
// render observed vehicles
if (!this.DesignMode && RemoraCommon.Communicator.GetObservedVehicles() != null)
{
SceneEstimatorTrackedClusterCollection setcc = RemoraCommon.Communicator.GetObservedVehicles();
for (int i = 0; i < setcc.clusters.Length; i++)
{
SensedVehicleDisplay svd = new SensedVehicleDisplay(setcc.clusters[i]);
svd.Render(e.Graphics, this.WorldTransform);
}
}
if (!this.DesignMode)
{
// render driver side sick
VehicleState state = RemoraCommon.Communicator.GetVehicleState();
if (state != null)
{
Coordinates dvec = state.Front - state.Position;
Coordinates driverVec = dvec.Rotate90();
SideObstacles dsobs = RemoraCommon.Communicator.GetSideObstacles(SideObstacleSide.Driver);
if (dsobs != null)
{
foreach (SideObstacle so in dsobs.obstacles)
{
if (so.height > 0.7)
{
Coordinates cVec = state.Position + driverVec.Normalize(so.distance);
DrawingUtility.DrawControlPoint(cVec, Color.Black, null, ContentAlignment.MiddleCenter, ControlPointStyle.LargeX, e.Graphics, this.WorldTransform);
}
}
}
// render driver side sick
Coordinates pvec = state.Front - state.Position;
Coordinates passVec = dvec.RotateM90();
SideObstacles psobs = RemoraCommon.Communicator.GetSideObstacles(SideObstacleSide.Passenger);
if (psobs != null)
{
foreach (SideObstacle so in psobs.obstacles)
{
if (so.height > 0.7)
{
Coordinates cVec = state.Position + passVec.Normalize(so.distance);
DrawingUtility.DrawControlPoint(cVec, Color.Black, null, ContentAlignment.MiddleCenter, ControlPointStyle.LargeX, e.Graphics, this.WorldTransform);
}
}
}
}
}
// render observed obstacles
if (!this.DesignMode && RemoraCommon.Communicator.GetVehicleState() != null && RemoraCommon.RoadNetwork != null)
{
// set and render
obstacleDisplay.untrackedClusters = RemoraCommon.Communicator.GetObservedObstacles();
obstacleDisplay.trackedClusters = RemoraCommon.Communicator.GetObservedVehicles();
obstacleDisplay.Render(e.Graphics, this.WorldTransform);
}
// restore the graphics state
e.Graphics.Restore(gs);
}
