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 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);
}
private static double FindBestCurvature(double prevCurvature, Coordinates goalPoint)
{
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer();
Coordinates relativeGoalPoint = absTransform.TransformPoint(goalPoint);
// get a list of obstacles
ObstacleCollection obstacles = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp, UrbanChallenge.Behaviors.SAUDILevel.None);
List <Polygon> obstaclePolygons = new List <Polygon>();
foreach (Obstacle obs in obstacles.obstacles)
{
obstaclePolygons.Add(obs.cspacePolygon);
}
// get the side obstacles
SideObstacle leftSideObstacle = Services.ObstaclePipeline.GetLeftSideObstacle();
SideObstacle rightSideObstacle = Services.ObstaclePipeline.GetRightSideObstacle();
double?leftDist = null, rightDist = null;
if (leftSideObstacle != null)
{
leftDist = leftSideObstacle.distance;
}
if (rightSideObstacle != null)
{
rightDist = rightSideObstacle.distance;
}
double roll = Services.Dataset.ItemAs <double>("roll").CurrentValue;
double roadBearing, roadConfidence;
RoadBearing.GetCurrentData(out roadBearing, out roadConfidence);
List <ArcResults> arcs = new List <ArcResults>();
double maxUtility = double.MinValue;
ArcResults selectedArc = null;
// recalculate weights
double totalWeights = obstacle_weight + hysteresis_weight + straight_weight + goal_weight + side_obs_weight + roll_weight + road_weight;
double obstacleWeight = obstacle_weight / totalWeights;
double hysteresisWeight = hysteresis_weight / totalWeights;
double straightWeight = straight_weight / totalWeights;
double goalWeight = goal_weight / totalWeights;
double sideObsWeight = side_obs_weight / totalWeights;
double rollWeight = roll_weight / totalWeights;
double roadWeight = road_weight / totalWeights;
int start = num_arcs / 2;
double curvatureStep = max_curvature / start;
for (int i = -start; i <= start; i++)
{
double curvature = i * curvatureStep;
double collisionDist, clearanceDist, collisionUtility;
bool vetoed;
EvaluateObstacleUtility(curvature, 20, obstaclePolygons, out collisionDist, out clearanceDist, out collisionUtility, out vetoed);
double hystersisUtility = EvaluateHysteresisUtility(curvature, prevCurvature);
double straightUtility = EvaluateStraightUtility(curvature);
double goalUtility = EvaluateGoalUtility(curvature, relativeGoalPoint);
double sideObstacleUtility = EvalualteSideObstacleUtility(curvature, leftDist, rightDist);
double rollUtility = EvaluateRollUtility(curvature, roll);
double roadUtility = EvaluateRoadBearingUtility(curvature, roadBearing, roadConfidence);
double totalUtility = collisionUtility * obstacleWeight + hystersisUtility * hysteresisWeight + straightUtility * straightWeight +
goalUtility * goalWeight + sideObstacleUtility * sideObsWeight + rollUtility * rollWeight + roadUtility * roadWeight;
ArcResults result = new ArcResults();
result.curvature = curvature;
result.vetoed = vetoed;
result.totalUtility = totalUtility;
result.obstacleHitDistance = collisionDist;
result.obstacleClearanceDistance = clearanceDist;
result.obstacleUtility = collisionUtility;
result.hysteresisUtility = hystersisUtility;
result.straightUtility = straightUtility;
result.goalUtility = goalUtility;
result.sideObstacleUtility = sideObstacleUtility;
result.rollUtility = rollUtility;
result.roadUtility = roadUtility;
arcs.Add(result);
if (!vetoed && totalUtility > maxUtility)
{
maxUtility = totalUtility;
selectedArc = result;
}
}
ArcVotingResults results = new ArcVotingResults();
results.arcResults = arcs;
results.selectedArc = selectedArc;
Services.Dataset.ItemAs <ArcVotingResults>("arc voting results").Add(results, LocalCarTimeProvider.LocalNow);
if (selectedArc == null)
{
return(double.NaN);
}
else
{
return(selectedArc.curvature);
}
}
