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