def generate_new_vehicle(self, vehicleClass):
"""Generate and import a new vehicle that will be controlled by SUMO."""
# load the new vehicle
vehicleString, defName = Vehicle.generate_vehicle_string(self.vehicleNumber, vehicleClass)
self.rootChildren.importMFNodeFromString(-1, vehicleString)
self.vehicles[self.vehicleNumber] = Vehicle(self.getFromDef(defName))
self.vehicleNumber += 1
def get_vehicle_index(self, id, generateIfneeded=True):
"""Look for the vehicle index corresponding to this id (and optionnaly create it if required)."""
for i in range(0, self.vehicleNumber):
if self.vehicles[i].currentID == id:
# the vehicle was already here at last step
return i
if not generateIfneeded:
return -1
# the vehicle was not present last step
# check if a corresponding vehicle is already in the simulation
node = self.getFromDef(id)
if node and (node.getTypeName() in Vehicle.get_car_models_list() or
node.getTypeName() in Vehicle.get_bus_models_list() or
node.getTypeName() in Vehicle.get_truck_models_list() or
node.getTypeName() in Vehicle.get_motorcycle_models_list()):
self.vehicles[self.vehicleNumber] = Vehicle(node)
self.vehicles[self.vehicleNumber].currentID = id
self.vehicleNumber += 1
return (self.vehicleNumber - 1)
# check if a vehicle is available
vehicleClass = self.get_vehicle_class(id)
for i in range(0, self.vehicleNumber):
if not self.vehicles[i].inUse and self.vehicles[i].vehicleClass == vehicleClass:
# if a vehicle is available assign it to this id
self.vehicles[i].currentID = id
self.vehicles[i].name.setSFString(id)
return i
# no vehicle available => generate a new one if limit is not reached
if self.vehicleNumber < self.vehiclesLimit:
vehicleClass = self.get_vehicle_class(id)
self.generate_new_vehicle(vehicleClass)
return (self.vehicleNumber - 1)
else:
return -1
def get_vehicle_class(self, id):
"""Get the class of the vehicle associated to this id."""
if id in self.vehiclesClass:
return self.vehiclesClass[id]
vehicleClass = Vehicle.get_corresponding_vehicle_class(self.traci.vehicle.getVehicleClass(id))
self.vehiclesClass[id] = vehicleClass
return vehicleClass
def get_initial_vehicles(self):
"""Get all the vehicles (both controlled by SUMO and Webots) already present in the world."""
for i in range(0, self.vehiclesLimit):
defName = "SUMO_VEHICLE%d" % self.vehicleNumber
node = self.getFromDef(defName)
if node:
self.vehicles[i] = Vehicle(node)
self.vehicles[i].name.setSFString("SUMO vehicle %i" % self.vehicleNumber)
self.vehicleNumber += 1
else:
break
for i in range(0, self.vehiclesLimit):
defName = "WEBOTS_VEHICLE%d" % self.webotsVehicleNumber
node = self.getFromDef(defName)
if node:
self.webotsVehicles[i] = WebotsVehicle(node, self.webotsVehicleNumber)
self.webotsVehicleNumber += 1
else:
break
def get_vehicles_position(self, subscriptionResult, step, xOffset, yOffset,
maximumLateralSpeed, maximumAngularSpeed, laneChangeDelay):
"""Compute the new desired position and orientation for all the vehicles controlled by SUMO."""
for id in subscriptionResult.keys():
height = 0.4
roll = 0.0
pitch = 0.0
sumoPos = subscriptionResult[id][self.traci.constants.VAR_POSITION]
sumoAngle = subscriptionResult[id][self.traci.constants.VAR_ANGLE]
pos = [-sumoPos[0] + xOffset, height, sumoPos[1] - yOffset]
angle = math.pi * sumoAngle / 180
dx = -math.cos(angle)
dy = -math.sin(angle)
yaw = -math.atan2(dy, -dx)
# correct position (origin of the car is not the same in Webots / sumo)
vehicleLength = subscriptionResult[id][self.traci.constants.VAR_LENGTH]
pos[0] += 0.5 * vehicleLength * math.sin(angle)
pos[2] -= 0.5 * vehicleLength * math.cos(angle)
# if needed check the vehicle is in the visibility radius
if self.radius > 0:
viewpointPosition = self.viewpointPosition.getSFVec3f()
xDiff = viewpointPosition[0] - pos[0]
yDiff = viewpointPosition[1]
zDiff = viewpointPosition[2] - pos[2]
distance = math.sqrt(xDiff * xDiff + yDiff * yDiff + zDiff * zDiff)
if distance > self.radius:
index = self.get_vehicle_index(id, generateIfneeded=False)
if index >= 0:
self.vehicles[index].inUse = False
self.vehicles[index].currentID = ""
self.vehicles[index].name.setSFString("SUMO vehicle %i" % index)
continue
index = self.get_vehicle_index(id)
if index >= 0:
vehicle = self.vehicles[index]
height = vehicle.wheelRadius
if self.enableHeight:
roadID = subscriptionResult[id][self.traci.constants.VAR_ROAD_ID]
roadPos = subscriptionResult[id][self.traci.constants.VAR_LANEPOSITION]
if roadID.startswith(":"):
# this is a lane change it does not contains edge information
# in that case, use previous height, roll and pitch
height = vehicle.currentPos[1]
roll = vehicle.roll
pitch = vehicle.pitch
else:
tags = roadID.split('_')
del tags[0] # remove the first one which is the 'id' of the road
for tag in tags:
if tag.startswith('height'):
height = height + float(tag.split('height', 1)[1])
elif tag.startswith('roll'):
roll = float(tag.split('roll', 1)[1])
elif tag.startswith('pitch'):
pitch = float(tag.split('pitch', 1)[1])
vehicle.pitch = pitch
vehicle.roll = roll
# ajust height according to the pitch
if not pitch == 0:
height += (roadPos - 0.5 * vehicleLength) * math.sin(pitch)
# ajust height according to the roll and lateral position of the vehicle
if not roll == 0.0:
laneIndex = subscriptionResult[id][self.traci.constants.VAR_LANE_INDEX]
laneID = subscriptionResult[id][self.traci.constants.VAR_LANE_ID]
laneWidth = self.traci.lane.getWidth(laneID)
edge = self.net.getEdge(roadID)
numberOfLane = edge.getLaneNumber()
# compute lateral distance from the center of the lane
distance = math.fabs((laneIndex - numberOfLane / 2) + 0.5) * laneWidth
if laneIndex >= (numberOfLane / 2):
height = height - distance * math.sin(roll)
else:
height = height + distance * math.sin(roll)
pos[1] = height
if vehicle.inUse:
# TODO: once the lane change model of SUMO has been improved
# (sub-lane model currently in development phase) we will be able to remove this corrections
# compute lateral (x) and longitudinal (z) displacement
diffX = pos[0] - vehicle.targetPos[0]
diffZ = pos[2] - vehicle.targetPos[2]
x1 = math.cos(-angle) * diffX - math.sin(-angle) * diffZ
z1 = math.sin(-angle) * diffX + math.cos(-angle) * diffZ
# check for lane change
if (vehicle.currentRoad is not None and
vehicle.currentRoad == subscriptionResult[id][self.traci.constants.VAR_ROAD_ID] and
vehicle.currentLane is not None and
vehicle.currentLane != subscriptionResult[id][self.traci.constants.VAR_LANE_INDEX]):
vehicle.laneChangeStartTime = self.getTime()
vehicle.laneChangeDistance = x1
x2 = x1
# artificially add an angle depending on the lateral speed
artificialAngle = 0
if z1 > 0.0001: # don't add the angle if speed is very small as atan2(0.0, 0.0) is unstable
# the '0.15' factor was found empirically and should not depend on the simulation
artificialAngle = 0.15 * math.atan2(x1, z1)
if (vehicle.laneChangeStartTime is not None and
vehicle.laneChangeStartTime > self.getTime() - laneChangeDelay and
abs(vehicle.laneChangeDistance) >= abs(x1)): # lane change case
ratio = (self.getTime() - vehicle.laneChangeStartTime) / laneChangeDelay
ratio = (0.5 + 0.5 * math.sin((ratio - 0.5) * math.pi))
p = vehicle.laneChangeDistance * ratio
x2 = x1 - (vehicle.laneChangeDistance - p)
artificialAngle = math.atan2(x2, z1)
# limit lateral speed
threshold = 0.001 * step * maximumLateralSpeed
x2 = min(max(x2, -threshold), threshold)
x3 = math.cos(angle) * x2 - math.sin(angle) * z1
z3 = math.sin(angle) * x2 + math.cos(angle) * z1
pos = [x3 + vehicle.targetPos[0], pos[1], z3 + vehicle.targetPos[2]]
diffYaw = yaw - vehicle.targetAngles[1] - artificialAngle
# limit angular speed
while diffYaw > math.pi:
diffYaw -= 2 * math.pi
while diffYaw < -math.pi:
diffYaw += 2 * math.pi
threshold = 0.001 * step * maximumAngularSpeed
diffYaw = min(max(diffYaw, -threshold), threshold)
yaw = diffYaw + vehicle.targetAngles[1]
# tilt motorcycle depending on the angluar speed
if vehicle.type in Vehicle.get_motorcycle_models_list():
threshold = 0.001 * step * maximumLateralSpeed
roll -= min(max(diffYaw / (0.001 * step), -0.2), 0.2)
rot = rotation_from_yaw_pitch_roll(yaw, pitch, roll)
if not vehicle.inUse:
# this vehicle was previously not used, move it directly to the correct initial location
vehicle.inUse = True
vehicle.currentPos = pos
vehicle.currentRot = rot
vehicle.currentAngles = [roll, yaw, pitch]
else:
vehicle.currentPos = vehicle.targetPos
vehicle.currentRot = vehicle.targetRot
vehicle.currentAngles = vehicle.targetAngles
# update target and wheels speed
vehicle.targetPos = pos
vehicle.targetRot = rot
vehicle.targetAngles = [roll, yaw, pitch]
if self.traci.constants.VAR_SPEED in subscriptionResult[id]:
vehicle.speed = subscriptionResult[id][self.traci.constants.VAR_SPEED]
vehicle.currentRoad = subscriptionResult[id][self.traci.constants.VAR_ROAD_ID]
vehicle.currentLane = subscriptionResult[id][self.traci.constants.VAR_LANE_INDEX]