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