/// <summary>
/// World service
/// </summary>
/// <param name="arn"></param>
public WorldService(ArbiterRoadNetwork arn)
{
this.RoadNetwork = arn;
this.worldState = new WorldState();
this.lidar = new SimSensor(Math.PI/3, -Math.PI/3, 0.5*Math.PI/180.0, SensorType.Scan, 50);
this.lidar2 = new SimSensor(4 * Math.PI / 3, 2 * Math.PI / 3, 0.5 * Math.PI / 180.0, SensorType.Scan, 30);
}
/// <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> /// Update the client /// </summary> public abstract SimVehicleState Update(WorldState worldState, double dt);
/// <summary>
/// Updates the world given a new world state from the simulation
/// </summary>
/// <param name="worldState"></param>
public void UpdateWorld(WorldState worldState)
{
this.worldState = worldState;
}
/// <summary>
/// Update the state of the client vehicle
/// </summary>
/// <param name="worldState"></param>
/// <param name="dt"></param>
public override SimVehicleState Update(WorldState worldState, double dt)
{
try
{
sumDt += dt;
// update world
this.world.UpdateWorld(worldState);
// update value held in sim client
this.ClientVehicle.Update(worldState);
// set vehicle speed check
double speedLock = this.ClientVehicle.CurrentState.Speed;
// vehicle state
VehicleState vs = world.VehicleStateFromSim(this.ClientVehicle.CurrentState);
vs.Timestamp = GetCurrentTimestamp.ts;
// update in ai and all listeners
this.communicator.Update(
vs,
world.VehiclesFromWorld(this.ClientVehicle.CurrentState.VehicleID, vs.Timestamp),
world.ObstaclesFromWorld(this.ClientVehicle.CurrentState.VehicleID, this.ClientVehicle.CurrentState.Position, this.ClientVehicle.CurrentState.Heading.ArcTan, vs.Timestamp),
this.ClientVehicle.CurrentState.Speed,
world.GetLocalRoadEstimate(this.ClientVehicle.CurrentState),
world.GetPathRoadEstimate(this.ClientVehicle.CurrentState));
// check if we're in step mode
lock (runControlClients) {
if (stepMode) {
foreach (ClientRunControlFacade stepClient in runControlClients.Values) {
stepClient.Step();
}
}
}
// save persistent info
double maxSpeed = this.ClientVehicle.CurrentState.MaximumSpeed;
bool useMaxSpeed = this.ClientVehicle.CurrentState.UseMaximumSpeed;
Coordinates pos = this.ClientVehicle.CurrentState.Position;
Coordinates heading = this.ClientVehicle.CurrentState.Heading;
bool canMove = this.ClientVehicle.CurrentState.canMove;
// get next state
DynamicsVehicle.VehicleState = this.ClientVehicle.CurrentState;
SimVehicleState updatedState = DynamicsVehicle.Update(dt, this.world);
// modify with persistent info
if (useMaxSpeed) {
updatedState.Speed = maxSpeed;
}
if (!canMove) {
updatedState.Position = pos;
updatedState.Heading = heading;
}
// set state
this.ClientVehicle.CurrentState = updatedState;
if(this.ClientVehicle.CurrentState.LockSpeed)
this.ClientVehicle.CurrentState.Speed = speedLock;
// return updated to sim
return updatedState;
}
catch (Exception e)
{
Console.WriteLine("Error updating: \n" + e.ToString());
return null;
}
}
/// <summary>
/// Updates the world given a new world state from the simulation
/// </summary>
/// <param name="worldState"></param>
public void UpdateWorld(WorldState worldState)
{
this.worldState = worldState;
}