/// <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); }