/// <summary> /// Distance to the next stop line /// </summary> /// <param name="simVehicleState"></param> /// <returns></returns> public double? DistanceToNextStop(SimVehicleState simVehicleState) { if (this.RoadNetwork == null) return null; else { IAreaSubtypeId iasi = this.GetAreaId(simVehicleState); if (iasi == null) return null; else if (iasi is ArbiterPerimeterId) return null; else { ArbiterLaneId ali = (ArbiterLaneId)iasi; // get closest ArbiterLanePartition alp = this.RoadNetwork.ArbiterSegments[ali.SegmentId].Lanes[ali].GetClosestPartition(simVehicleState.Position); ArbiterWaypoint waypoint; double distance; RoadNetworkTools.NextStop(alp.Lane, alp, simVehicleState.Position, out waypoint, out distance); if (waypoint == null) return null; else return distance; } } }
private double throttle; // throttle (between 0 and 1), the fraction of maximum torque commanded #endregion Fields #region Constructors public DynamicsSimVehicle(SimVehicleState vehicleState, ObjectDirectory od) { this.simVehicleState = vehicleState; InitializeState(); Connect(od); }
/// <summary> /// Update the vehicle /// </summary> /// <param name="worldState"></param> public void Update(WorldState worldState) { // update the current state held in the sim this.CurrentState = worldState.Vehicles[VehicleId]; }
/// <summary> /// COnstructor /// </summary> /// <param name="state"></param> /// <param name="length"></param> /// <param name="width"></param> public SimVehicle(SimVehicleState state, double length, double width) { this.VehicleState = state; this.VehicleState.Length = length; this.VehicleState.Width = width; }
/// <summary> /// COnstructor /// </summary> /// <param name="state"></param> /// <param name="length"></param> /// <param name="width"></param> public SimVehicle(SimVehicleState state, double length, double width) { this.VehicleState = state; this.VehicleState.Length = length; this.VehicleState.Width = width; }
private SceneEstimatorClusterPartition[] GetClusterPartitions(SimVehicleState svs, SimVehicleState ours) { List<SceneEstimatorClusterPartition> partitions = new List<SceneEstimatorClusterPartition>(); foreach(IVehicleArea iva in this.RoadNetwork.VehicleAreas) { if (ours.Position.DistanceTo(svs.Position) < 55) { if (iva.ContainsVehicle(svs.Position, svs.Length, svs.Width, svs.Heading)) { SceneEstimatorClusterPartition secp = new SceneEstimatorClusterPartition(); secp.partitionID = iva.DefaultAreaId(); secp.probability = 1; partitions.Add(secp); } } else { if (iva.ContainsVehicle(svs.Position, svs.Length, svs.Width * 4.0, svs.Heading)) { SceneEstimatorClusterPartition secp = new SceneEstimatorClusterPartition(); secp.partitionID = iva.DefaultAreaId(); secp.probability = 1; partitions.Add(secp); } } } return partitions.ToArray(); }
/// <summary> /// Make a vehicle state from a sim vehicle state /// </summary> /// <param name="simState"></param> /// <returns></returns> public VehicleState VehicleStateFromSim(SimVehicleState simState) { // vehicle state VehicleState vs = new VehicleState(); // area vs.Area = new List<AreaEstimate>(); // ae AreaEstimate ae = new AreaEstimate(); ae.Probability = 1; // get string id string id = ""; // set id ae.AreaId = id; ae.AreaType = StateAreaType.Interconnect; // get area IAreaSubtypeId iasi = this.GetAreaId(simState); if (iasi is ArbiterPerimeterId) { id = ((ArbiterPerimeterId)iasi).ToString(); ae.AreaType = StateAreaType.Zone; } else if (iasi is ArbiterLaneId) { ae.AreaId = ((ArbiterLaneId)iasi).ToString() + ".1"; ae.AreaType = StateAreaType.Lane; } // add ae vs.Area.Add(ae); // set others vs.Heading = simState.Heading; vs.Position = simState.Position; // return return vs; }
/// <summary> /// Gets area id of a vehicle's integer area id /// </summary> /// <param name="vehicleState"></param> /// <returns></returns> public IAreaSubtypeId GetAreaId(SimVehicleState vehicleState) { return RoadNetworkTools.GetClosest(this.RoadNetwork, vehicleState.Position); }
public Coordinates[] VehiclePointsRelative(SimVehicleState svs, Coordinates r, Coordinates h) { Coordinates[] cs = this.VehiclePoints(svs); for (int i = 0; i < cs.Length; i++) { cs[i] = cs[i] - r; cs[i] = cs[i].Rotate(-h.ArcTan); } return cs; }
public Polygon VehiclePolygon(SimVehicleState sos) { Coordinates headingOffset = sos.Heading.Rotate90(); Coordinates center = sos.Position + sos.Heading.Normalize(TahoeParams.FL - (sos.Length / 2.0)); List<Coordinates> coords = new List<Coordinates>(); coords.Add(center + sos.Heading.Normalize(sos.Length / 2.0) + headingOffset.Normalize(sos.Width / 2.0)); coords.Add(center + sos.Heading.Normalize(sos.Length / 2.0) - headingOffset.Normalize(sos.Width / 2.0)); coords.Add(center - sos.Heading.Normalize(sos.Length / 2.0) + headingOffset.Normalize(sos.Width / 2.0)); coords.Add(center - sos.Heading.Normalize(sos.Length / 2.0) - headingOffset.Normalize(sos.Width / 2.0)); Polygon p = new Polygon(coords, CoordinateMode.AbsoluteProjected); return p; }
public Coordinates[] VehiclePoints(SimVehicleState svs) { Polygon p = VehiclePolygon(svs); LinePath lp = new LinePath(p); LinePath.PointOnPath c = lp.StartPoint; List<Coordinates> cs = new List<Coordinates>(); while (!c.Equals(lp.EndPoint)) { cs.Add(lp.GetPoint(c)); c = lp.AdvancePoint(c, 0.1); } return cs.ToArray(); }
public PathRoadModel GetPathRoadEstimate(SimVehicleState state) { PathRoadModel ret = new PathRoadModel(new List<PathRoadModel.LaneEstimate>(), (double)SimulatorClient.GetCurrentTimestamp); ArbiterLanePartition closestPartition = this.RoadNetwork.ClosestLane(state.Position).GetClosestPartition(state.Position); LinePath path; if (IsPartitionSameDirection(closestPartition, state.Heading)) { path = BuildPath(closestPartition, true, 10, 40); } else { path = BuildPath(closestPartition, false, 10, 40); } Matrix3 absTransform = Matrix3.Rotation(-state.Heading.ArcTan)*Matrix3.Translation(-state.Position.X, -state.Position.Y); path.TransformInPlace(absTransform); PathRoadModel.LaneEstimate center = new PathRoadModel.LaneEstimate(path, closestPartition.Lane.Width, closestPartition.PartitionId.ToString()); ret.laneEstimates.Add(center); // get the lane to the left if (closestPartition.Lane.LaneOnLeft != null) { ArbiterLanePartition partition = closestPartition.Lane.LaneOnLeft.GetClosestPartition(state.Position); if (closestPartition.NonLaneAdjacentPartitions.Contains(partition)) { if (IsPartitionSameDirection(partition, state.Heading)) { path = BuildPath(partition, true, 10, 25); } else { path = BuildPath(partition, false, 10, 25); } path.TransformInPlace(absTransform); PathRoadModel.LaneEstimate left = new PathRoadModel.LaneEstimate(path, partition.Lane.Width, partition.PartitionId.ToString()); ret.laneEstimates.Add(left); } } if (closestPartition.Lane.LaneOnRight != null) { ArbiterLanePartition partition = closestPartition.Lane.LaneOnRight.GetClosestPartition(state.Position); if (closestPartition.NonLaneAdjacentPartitions.Contains(partition)) { if (IsPartitionSameDirection(partition, state.Heading)) { path = BuildPath(partition, true, 10, 25); } else { path = BuildPath(partition, false, 10, 25); } path.TransformInPlace(absTransform); PathRoadModel.LaneEstimate right = new PathRoadModel.LaneEstimate(path, partition.Lane.Width, partition.PartitionId.ToString()); ret.laneEstimates.Add(right); } } return ret; }
public LocalRoadEstimate GetLocalRoadEstimate(SimVehicleState state) { LocalRoadEstimate lre = new LocalRoadEstimate(); /*ArbiterLanePartition closestPartition = this.RoadNetwork.ClosestLane(state.Position).GetClosestPartition(state.Position); LinePath path; if (IsPartitionSameDirection(closestPartition, state.Heading)) { path = BuildPath(closestPartition, true, 25, 25); } else { path = BuildPath(closestPartition, false, 25, 25); } Matrix3 absTransform = Matrix3.Rotation(-state.Heading.ArcTan)*Matrix3.Translation(-state.Position.X, -state.Position.Y); path.TransformInPlace(absTransform); // get the closest point LinePath.PointOnPath p0 = path.ZeroPoint; List<Coordinates> points = new List<Coordinates>(5); List<double> dists = new List<double>(); dists.Add(0); points.Add(path.GetPoint(p0)); // iterate through and add points every five meteres LinePath.PointOnPath pt = p0; double sumDist = 0; for (int i = 0; i < 3; i++) { double dist = 5; pt = path.AdvancePoint(pt, ref dist); if (dist != 5) { sumDist += (5-dist); dists.Add(sumDist); points.Add(path.GetPoint(pt)); } if (dist > 0) break; } pt = p0; sumDist = 0; for (int i = 0; i < 2; i++) { double dist = -5; pt = path.AdvancePoint(pt, ref dist); if (dist != -5) { sumDist += (5+dist); dists.Insert(0, sumDist); points.Insert(0, path.GetPoint(pt)); } if (dist < 0) break; } // do a least squares fit over the points Matrix X = new Matrix(points.Count, 3); Matrix W = new Matrix(points.Count, points.Count); Matrix Y = new Matrix(points.Count, 1); for (int i = 0; i < points.Count; i++) { X[i, 0] = 1; X[i, 1] = points[i].X; X[i, 2] = points[i].X*points[i].X; W[i, i] = 1/(dists[i] + 1); Y[i, 0] = points[i].Y; } Matrix beta = null; try { Matrix X_T = X.Transpose(); Matrix inv = (X_T*W*X).Inverse; beta = inv*X_T*W*Y; } catch (Exception) { } if (beta != null) { lre.isModelValid = true; lre.roadCurvature = beta[2, 0]; lre.roadCurvatureVar = double.MaxValue; lre.roadHeading = beta[1, 0]; lre.roadHeadingVar = 0; lre.centerLaneEstimate = new UrbanChallenge.Common.Sensors.LocalRoadEstimate.LaneEstimate(); lre.centerLaneEstimate.center = beta[0, 0]; lre.centerLaneEstimate.centerVar = 0; lre.centerLaneEstimate.exists = true; lre.centerLaneEstimate.id = closestPartition.PartitionId.ToString(); lre.centerLaneEstimate.width = closestPartition.Lane.Width; lre.centerLaneEstimate.widthVar = 0; lre.leftLaneEstimate = new UrbanChallenge.Common.Sensors.LocalRoadEstimate.LaneEstimate(); lre.leftLaneEstimate.exists = false; if (closestPartition.Lane.LaneOnLeft != null) { ArbiterLanePartition leftParition = closestPartition.Lane.LaneOnLeft.GetClosestPartition(state.Position); if (closestPartition.NonLaneAdjacentPartitions.Contains(leftParition)) { double dist; if (IsPartitionSameDirection(leftParition, state.Heading)) { dist = (state.Position-leftParition.Initial.Position).Cross(leftParition.Vector().Normalize()); } else { dist = (state.Position-leftParition.Final.Position).Cross(leftParition.Vector().Rotate180().Normalize()); } lre.leftLaneEstimate.center = dist; lre.leftLaneEstimate.centerVar = 0; lre.leftLaneEstimate.exists = true; lre.leftLaneEstimate.id = leftParition.PartitionId.ToString(); lre.leftLaneEstimate.width = leftParition.Lane.Width; lre.leftLaneEstimate.widthVar = 0; } } if (closestPartition.Lane.LaneOnRight != null) { ArbiterLanePartition rightPartition = closestPartition.Lane.LaneOnRight.GetClosestPartition(state.Position); if (closestPartition.NonLaneAdjacentPartitions.Contains(rightPartition)) { double dist; if (IsPartitionSameDirection(rightPartition, state.Heading)) { dist = (state.Position-rightPartition.Initial.Position).Cross(rightPartition.Vector().Normalize()); } else { dist = (state.Position-rightPartition.Final.Position).Cross(rightPartition.Vector().Rotate180().Normalize()); } lre.rightLaneEstimate.center = dist; lre.rightLaneEstimate.centerVar = 0; lre.rightLaneEstimate.exists = true; lre.rightLaneEstimate.id = rightPartition.PartitionId.ToString(); lre.rightLaneEstimate.width = rightPartition.Lane.Width; lre.rightLaneEstimate.widthVar = 0; } } } else { lre.isModelValid = false; }*/ lre.centerLaneEstimate.exists = false; lre.leftLaneEstimate.exists = false; lre.rightLaneEstimate.exists = false; lre.stopLineEstimate = new StopLineEstimate(); double? distToStopline = DistanceToNextStop(state); lre.stopLineEstimate.stopLineExists = distToStopline.HasValue; lre.stopLineEstimate.distToStopline = distToStopline.GetValueOrDefault(double.MaxValue); lre.stopLineEstimate.distToStoplineVar = 0; lre.timestamp = (double)SimulatorClient.GetCurrentTimestamp; return lre; }
/// <summary> /// Adds a vehicle to the world /// </summary> /// <param name="screenCenter"></param> /// <returns></returns> public SimVehicle AddVehicle(Coordinates screenCenter) { // deafualt id is # of elts int idNumber = this.WorldService.Obstacles.Count + this.Vehicles.Count; // create array to hold used vehicle and obstacle id's bool[] usedIds = new bool[this.WorldService.Obstacles.Count + this.Vehicles.Count]; // loop over obstacles and get ids foreach (SimObstacle ism in this.WorldService.Obstacles.Values) { // make sure num within # of obstacles and vehicles if (ism.ObstacleId.Number < this.WorldService.Obstacles.Count + this.Vehicles.Count) { // check off usedIds[ism.ObstacleId.Number] = true; } } // loop over vehicles and get ids foreach (SimVehicle ism in this.Vehicles.Values) { // make sure num within # of obstacles and vehicles if (ism.VehicleId.Number < this.WorldService.Obstacles.Count + this.Vehicles.Count) { // check off usedIds[ism.VehicleId.Number] = true; } } // loop over checked off id's for (int i = usedIds.Length - 1; i >= 0; i--) { // if find a false one set that id if (usedIds[i] == false) { // set id num idNumber = i; } } // create vehicle id SimVehicleId svi = new SimVehicleId(idNumber); // get position (center of screen) Coordinates position = screenCenter; // create state SimVehicleState svs = new SimVehicleState(); svs.canMove = true; svs.Heading = new Coordinates(1, 0); svs.IsBound = false; svs.Position = position; svs.Speed = 0; svs.VehicleID = svi; // create vehicle SimVehicle stv = new SimVehicle(svs, TahoeParams.VL, TahoeParams.T); // add to vehicles this.Vehicles.Add(stv.VehicleId, stv); // return return(stv); }
private AreaEstimate[] GetAreaEstimates(SimVehicleState svs) { List<AreaEstimate> estimates = new List<AreaEstimate>(); foreach (IVehicleArea iva in this.RoadNetwork.VehicleAreas) { if (iva.ContainsVehicle(svs.Position, svs.Length, svs.Width, svs.Heading)) { AreaEstimate ae = new AreaEstimate(); ae.AreaId = iva.DefaultAreaId(); if(iva is ArbiterInterconnect) ae.AreaType = StateAreaType.Interconnect; else if(iva is ArbiterLane) ae.AreaType = StateAreaType.Lane; else ae.AreaType = StateAreaType.Zone; ae.Probability = 1; estimates.Add(ae); } } return estimates.ToArray(); }
/// <summary> /// The simulation main thread /// </summary> public void Simulate() { // run sim at 10Hz MMWaitableTimer timer = new MMWaitableTimer((uint)this.settings.SimCycleTime); // notify this.simulationMain.SimulationModeLabel.Text = "Simulation Running"; this.simulationMain.SimulationModeLabel.Image = global::Simulator.Properties.Resources.Light_Bulb_On_16_n_p; // run while on while (this.SimulationState == SimulationState.Running) { if (stepMode) { // if we're in step mode, then wait on the step event or time out bool gotEvent = stepEvent.WaitOne(250, false); // if we timed out, start the loop over if (!gotEvent) { continue; } } else { // wait for 10hz timer.WaitEvent.WaitOne(); } try { // get world state WorldState ws = this.WorldService.GetWorldState(); lock (this.simulationMain.clientHandler) { // set world state to each client in the sim foreach (string client in this.simulationMain.clientHandler.VehicleToClientMap.Values) { try { // update client SimVehicleState nextState = this.simulationMain.clientHandler.AvailableClients[client].Update(ws, (double)this.settings.SimCycleTime / 1000.0); if (nextState != null) { // update state this.Vehicles[this.simulationMain.clientHandler.ClientToVehicleMap[client]].SimVehicleState = nextState; } else { throw new Exception("Received null SimVehicleState from " + client); } } catch (Exception e) { if (!this.simulationMain.IsDisposed) { this.simulationMain.BeginInvoke(new MethodInvoker(delegate() { // notify SimulatorOutput.WriteLine("Error Updating Client: " + client + ", Simulation Stopped"); // set state this.simulationMain.SimulationModeLabel.Text = "Simulation Stopped"; this.simulationMain.SimulationModeLabel.Image = global::Simulator.Properties.Resources.Light_Bulb_Off_16_n_p; // stop this.EndSimulation(); })); Console.WriteLine(e.ToString()); // leave break; } } } } // redraw this.simulationMain.BeginInvoke(new MethodInvoker(delegate() { // notify this.simulationMain.roadDisplay1.Invalidate(); })); } catch (Exception e) { if (!this.simulationMain.IsDisposed) { this.simulationMain.BeginInvoke(new MethodInvoker(delegate() { // notify SimulatorOutput.WriteLine("Error in outer sim loop:" + e.ToString()); })); } } } }
/// <summary> /// Adds a vehicle to the world /// </summary> /// <param name="screenCenter"></param> /// <returns></returns> public SimVehicle AddVehicle(Coordinates screenCenter) { // deafualt id is # of elts int idNumber = this.WorldService.Obstacles.Count + this.Vehicles.Count; // create array to hold used vehicle and obstacle id's bool[] usedIds = new bool[this.WorldService.Obstacles.Count + this.Vehicles.Count]; // loop over obstacles and get ids foreach (SimObstacle ism in this.WorldService.Obstacles.Values) { // make sure num within # of obstacles and vehicles if (ism.ObstacleId.Number < this.WorldService.Obstacles.Count + this.Vehicles.Count) { // check off usedIds[ism.ObstacleId.Number] = true; } } // loop over vehicles and get ids foreach (SimVehicle ism in this.Vehicles.Values) { // make sure num within # of obstacles and vehicles if (ism.VehicleId.Number < this.WorldService.Obstacles.Count + this.Vehicles.Count) { // check off usedIds[ism.VehicleId.Number] = true; } } // loop over checked off id's for (int i = usedIds.Length - 1; i >= 0; i--) { // if find a false one set that id if (usedIds[i] == false) { // set id num idNumber = i; } } // create vehicle id SimVehicleId svi = new SimVehicleId(idNumber); // get position (center of screen) Coordinates position = screenCenter; // create state SimVehicleState svs = new SimVehicleState(); svs.canMove = true; svs.Heading = new Coordinates(1,0); svs.IsBound = false; svs.Position = position; svs.Speed = 0; svs.VehicleID = svi; // create vehicle SimVehicle stv = new SimVehicle(svs, TahoeParams.VL, TahoeParams.T); // add to vehicles this.Vehicles.Add(stv.VehicleId, stv); // return return stv; }