def call(port: int, req: Request) -> Response:
if req.function in [
'get_graph_recursive', 'intra_graph_call',
'route_intra_graph_call', 'tick', 'get_pose', 'get_simulation'
]:
req.longRunning = True
s = zmq.Context().socket(zmq.REQ)
if not req.longRunning:
s.setsockopt(zmq.RCVTIMEO, 10)
s.setsockopt(zmq.SNDTIMEO, 10)
s.setsockopt(zmq.LINGER, 0)
else:
s.setsockopt(zmq.RCVTIMEO, -1)
s.setsockopt(zmq.SNDTIMEO, -1)
s.setsockopt(zmq.LINGER, 1)
s.connect("tcp://localhost:{}".format(port))
s.send(req.serialize())
try:
recv = s.recv()
except zmq.error.Again as a:
raise a
finally:
s.close()
resp = Response.load(recv)
return resp
def test_findNodes():
N = 7
objs = []
coordDict = {}
for i in range(N):
a = Agent(ssid=f'AGENT-{str(i)}')
pos = Pose2d(trans=Translation2d(random(), random()),
rot=Rotation2d(1, 0))
coordDict[AgentRepresentation.fromAgent(a)] = [pos.x, pos.y]
MeshNode.call(a.portNumber, Request("set_pose", args=[pos]))
objs.append(a)
(graph, traversed) = MeshNode.call(
objs[0].portNumber,
Request('get_graph_recursive', args=[[]], longRunning=True)).response
print(f"Traversed total: {[i.ssid for i in traversed]}")
import matplotlib.pyplot as plt
# nx.draw_networkx(graph, pos=coordDict,
# labels={key: key.ssid for key in [AgentRepresentation.fromAgent(a) for a in objs]})
nx.draw_networkx(
graph,
pos=coordDict,
labels={
key: key.ssid
for key in [AgentRepresentation.fromAgent(a) for a in objs]
})
# nx.draw_random(graph)
# nx.draw_circular(graph)
# nx.draw_spectral(graph)
# nx.draw_planar(graph)
plt.show()
def findAgents(self):
"""
:return: All agents in SSID scan range, sorted by distance
"""
known_agents = []
for port in range(defaultPortRange[0], defaultPortRange[1]):
if port == self.portNumber:
continue
try:
# print(f"Trying port {port}... ", end='')
ssid = MeshNode.call(port, Request('ssid')).response
except zmq.error.Again:
# print(f"no agent found")
pass
else:
agentCoords: np.ndarray = MeshNode.call(
port, Request('get_pose')).response
dist = self._getDistance(agentCoords)
# print(f"agent found with coordinates {agentCoords} and distance {dist}.")
if dist <= DIST_THRESHHOLD:
known_agents.append(
AgentRepresentation(port=port, ssid=ssid))
return sorted(known_agents, key= \
lambda a: np.linalg.norm((MeshNode.call(a.port, Request(
'get_pose')).response.translation - self.vehiclePose.translation).position.asNDArray()))
def run(self):
self.world.tick()
n_tick = 0
car_b_x_o = self.car_b.carla_vehicle.get_location().x
last_tick_t = time()
while True:
if n_tick == 170:
self.car_a._setDrivingBehavior(
AgentDrivingBehavior.FOLLOW_WAYPOINTS)
if n_tick == 100:
self.car_b._setDrivingBehavior(
AgentDrivingBehavior.FOLLOW_WAYPOINTS)
n_tick += 1
self.world.tick()
if n_tick < 180:
self.car_a.velocityReference = 40
self.car_a.waypointFollowSpeed = 40
self.car_b.velocityReference = 30
self.car_b.waypointFollowSpeed = 30
if n_tick == 181:
physics_control = self.car_b.carla_vehicle.get_physics_control(
)
physics_control.mass = 40000
self.car_b.carla_vehicle.apply_physics_control(physics_control)
physics_control = self.car_a.carla_vehicle.get_physics_control(
)
physics_control.mass = 400
self.car_a.carla_vehicle.apply_physics_control(physics_control)
#if n_tick == 182:
# self.car_a._setDrivingBehavior(AgentDrivingBehavior.PASSIVE)
# self.car_b._setDrivingBehavior(AgentDrivingBehavior.PASSIVE)
car_b_loc = self.car_b.vehiclePose
MeshNode.call(self.car_b.portNumber,
Request('tick', args=[], longRunning=True))
MeshNode.call(self.car_a.portNumber,
Request('tick', args=[], longRunning=True))
print(f"Tick: {n_tick}")
self.car_b._setWaypoints(
gen_waypoints_straight_y(car_b_loc, car_b_x_o))
self.car_a._setWaypoints(hardcoded_cop_waypoints())
if n_tick == 1000:
break
print(f"dt: {time() - last_tick_t}")
dt = time() - last_tick_t
if dt < enforce_dt:
sleep(enforce_dt - dt)
last_tick_t = time()
def run(self):
self.world.tick()
n_tick = 0
start_y = self.car_a.carla_vehicle.get_location().y
lane_width = 4 # m
lane_change_dist = 10 # m
lane_change_x = None
while True:
n_tick += 1
self.world.tick()
MeshNode.call(self.car_a.portNumber,
Request('tick', args=[], longRunning=True))
car_a_loc = self.car_a.vehiclePose
if n_tick == 100:
self.car_a._setDrivingBehavior(
AgentDrivingBehavior.FOLLOW_WAYPOINTS)
print("Set to following waypoints mode")
if n_tick < 500:
self.car_a._setWaypoints(
gen_waypoints_straight_x(car_a_loc, start_y))
else:
if lane_change_x is None:
print(f"Lane change in {lane_change_dist} m")
lane_change_x = car_a_loc.x + lane_change_dist
self.car_a._setWaypoints(
gen_lanechange_waypoints(car_a_loc, start_y,
start_y + lane_width,
lane_change_x))
print(
f"n_tick: {n_tick}\tangular_vel: {self.car_a.angularVelocityReference}"
)
def spawn_vehicles(self):
self.car_a = Agent(ssid='car_a')
self.car_b = Agent(ssid='car_b')
self.car_a.connect_carla()
self.car_b.connect_carla()
# set up mesh network
MeshNode.call(
self.car_a.portNumber,
Request('get_graph_recursive', args=[[]], longRunning=True))
self.car_a.spawn_vehicle(x=45,
y=-36.5,
z=0.25,
yaw=180,
template_filter='*charger*')
self.car_b.spawn_vehicle(x=35,
y=-10,
z=0.25,
yaw=-90,
template_filter='*cola*')
self.car_a.overridespeed = True
self.car_a.driveController.max_steering_angle *= 2
#self.car_a.purePursuitEndWaypointDist = 1
self.car_a._setWaypoints(hardcoded_cop_waypoints())
def _thread(self):
while self.running:
request_b = self.socket.recv()
request = Request.load(request_b)
response = self.dispatch(request)
response_b = response.serialize()
self.socket.send(response_b)
def run(self):
self.world.tick()
n_tick = 0
car_b_x_o = self.car_b.carla_vehicle.get_location().x
last_tick_t = time()
self.car_b.drivingMode = AgentDrivingMode.DRIVING_CITY
self.car_a.drivingMode = AgentDrivingMode.ASSHOLE
while True:
if n_tick == 170:
self.car_a._setDrivingBehavior(
AgentDrivingBehavior.FOLLOW_WAYPOINTS)
if n_tick == 100:
self.car_b._setDrivingBehavior(
AgentDrivingBehavior.FOLLOW_WAYPOINTS)
n_tick += 1
self.world.tick()
self.car_a.velocityReference = 15
self.car_a.waypointFollowSpeed = 15
self.car_b.velocityReference = 30
self.car_b.waypointFollowSpeed = 30
if n_tick >= 170:
if self.car_b._willCollide(
AgentRepresentation.fromAgent(self.car_a)):
self.car_b.velocityReference = 0
self.car_b.waypointFollowSpeed = 0
else:
self.car_b.velocityReference = 30
self.car_b.waypointFollowSpeed = 30
car_b_loc = self.car_b.vehiclePose
MeshNode.call(self.car_b.portNumber,
Request('tick', args=[], longRunning=True))
MeshNode.call(self.car_a.portNumber,
Request('tick', args=[], longRunning=True))
print(f"Tick: {n_tick}")
self.car_b._setWaypoints(
gen_waypoints_straight_y(car_b_loc, car_b_x_o))
if n_tick == 600:
break
print(f"dt: {time() - last_tick_t}")
dt = time() - last_tick_t
if (dt < enforce_dt):
print(f"sleeping for: {enforce_dt-dt}")
sleep(enforce_dt - dt)
print(f"dt_adj: {time() - last_tick_t}")
last_tick_t = time()
def _distanceToFollowTarget(self) -> Translation2d:
our_pose = self.vehiclePose
their_pose: Pose2d = MeshNode.call(self.followTarget.port,
Request('get_pose')).response
pose_difference = their_pose.translation - our_pose.translation
diff_along_axis: float = pose_difference.position.dot(
self.followAxis.rotation)
return diff_along_axis
def negotiatePriority(self, other: AgentRepresentation):
ourMode = self.drivingMode
theirMode = MeshNode.call(other.port,
Request('get_driving_mode')).response
if ourMode.value > theirMode.value:
return other
else:
return AgentRepresentation.fromAgent(self)
def routeGraphRequest(self, req: Request, path: List[AgentRepresentation]):
if len(path) == 1:
return self.dispatch(req).response
else:
path.pop(0)
return MeshNode.call(path[0].port,
req=Request("route_intra_graph_call",
[req, path])).response
def run(self):
while True:
MeshNode.call(self.car_a.portNumber,
Request('tick', args=[[]], longRunning=True))
# MeshNode.call(self.car_b.portNumber, Request('tick', args=[[]], longRunning=True))
# MeshNode.call(self.car_c.portNumber, Request('tick', args=[[]], longRunning=True))
self.world.tick()
def _setMerge(self, frontCar: AgentRepresentation,
backCar: AgentRepresentation):
self.followTarget = frontCar
self.drivingBehavior = AgentDrivingBehavior.MERGING
MeshNode.call(
backCar.port,
Request("set_follow_target",
args=[AgentRepresentation.fromAgent(self)])).response
# self.mergeStartTime = time()
self.mergeStartTime = 0.0
def spawn_vehicles(self):
self.car_1 = Agent(ssid='car_a')
self.car_2 = Agent(ssid='car_b')
self.car_3 = Agent(ssid='car_c')
self.car_4 = Agent(ssid='car_d')
self.car_5 = Agent(ssid='car_e')
self.car_1.connect_carla()
self.car_2.connect_carla()
self.car_3.connect_carla()
self.car_4.connect_carla()
self.car_5.connect_carla()
# set up mesh network
self.car_1.spawn_vehicle(x=-46.8,
y=37.0,
z=0.50,
yaw=-90,
template_filter='*charger*')
self.car_2.spawn_vehicle(x=-13.65,
y=-1.00,
z=0.50,
yaw=180,
template_filter='*citroen*')
self.car_3.spawn_vehicle(x=-13.65,
y=-4.10,
z=0.50,
yaw=180,
template_filter='*grandtourer*')
self.car_4.spawn_vehicle(x=-54.9,
y=-34.8,
z=0.50,
yaw=90,
template_filter='*coupe*')
self.car_5.spawn_vehicle(x=-51.1,
y=-34.8,
z=0.50,
yaw=90,
template_filter='*a2*')
self.car_1.overridespeed = True
self.car_2.overridespeed = True
self.car_3.overridespeed = True
self.car_4.overridespeed = True
self.car_5.overridespeed = True
self.car_1.drivingMode = AgentDrivingMode.ASSHOLE
self.car_2.drivingMode = AgentDrivingMode.MERGING_REQ
self.car_3.drivingMode = AgentDrivingMode.MERGING_ACT
self.car_4.drivingMode = AgentDrivingMode.DRIVING_HIGHWAY
self.car_5.drivingMode = AgentDrivingMode.DRIVING_CITY
MeshNode.call(
self.car_1.portNumber,
Request('get_graph_recursive', args=[[]], longRunning=True))
def _frameUpdate(self):
# vel = self._getCarForwardVelocity()
trns = self.carla_vehicle.get_transform().location
rotsV = self.carla_vehicle.get_transform().rotation.get_forward_vector(
)
self.vehiclePose.translation = Translation2d(trns.x, trns.y)
self.vehiclePose.rotation = Rotation2d(rotsV.x, rotsV.y)
debug = False
dbg = self.carla_world.debug
for i in self.waypointList:
loc = carla.Location(i.x, i.y, 0.5)
if debug:
dbg.draw_point(loc, 0.1, carla.Color(0, 255, 0), 0.02)
pointPt = self.vehiclePose.translation + Translation2d(
5, 0).rotateByOrigin(self.vehiclePose.rotation)
loc = carla.Location(pointPt.x, pointPt.y, 0.5)
if debug:
dbg.draw_point(loc, 0.1, carla.Color(255, 0, 0), 0.01)
if len(self.waypointList) > 0:
loc = carla.Location(self.waypointList[0].x,
self.waypointList[0].y, 0.51)
if debug: dbg.draw_point(loc, 0.2, carla.Color(0, 0, 255), 0.01)
if self.drivingBehavior == AgentDrivingBehavior.FOLLOW_WAYPOINTS:
if self.collisionDetection:
a = self._getAnyCollisions()
if a is not None:
print(f"Solving for collision with {a.ssid}")
self._solveForProfileToAvoidCollision(a)
self.velocityReference = self.waypointFollowSpeed
self.angularVelocityReference = self._purePursuitAngleToAngularVelocity(
)
print(f"AM {self.name}, GOING {self.velocityReference}")
elif self.drivingBehavior == AgentDrivingBehavior.MAINTAIN_DISTANCE:
self.velocityReference = self._runFollowPDLoop()
self.angularVelocityReference = self._purePursuitAngleToAngularVelocity(
)
elif self.drivingBehavior == AgentDrivingBehavior.MERGING:
self.mergeStartTime += 0.01
if self.mergeStartTime >= self.mergeDwell:
self.drivingBehavior = AgentDrivingBehavior.MAINTAIN_DISTANCE
self.waypointList = deepcopy(
MeshNode.call(self.followTarget.port,
Request("get_waypoints")).response)
self.velocityReference = self._runFollowPDLoop()
self.angularVelocityReference = self._purePursuitAngleToAngularVelocity(
)
elif self.drivingBehavior == AgentDrivingBehavior.WAITING:
self.velocityReference = 0
self.angularVelocityReference = 0
else:
return
self.drive_vehicle()
def spawn_vehicles(self):
self.car_a = Agent(ssid='car_a')
self.car_a.connect_carla()
# set up mesh network
MeshNode.call(
self.car_a.portNumber,
Request('get_graph_recursive', args=[[]], longRunning=True))
self.car_a.spawn_vehicle(x=-205, y=-91.25, z=0.1, yaw=0)
def test_findSSIDs():
N = 10
objs = []
for i in range(N):
a = Agent(ssid=f'AGENT-{str(i)}')
MeshNode.call(
a.portNumber,
Request("set_pose", args=[np.asarray([random(), random()])]))
objs.append(a)
print(objs[0].findAgents())
def run(self):
self.world.tick()
n_tick = 0
# self.car_a.velocityReference =8.9408 * 2
start_y = self.car_a.carla_vehicle.get_location().y
car_a_loc = self.car_a.vehiclePose
self.car_a._setWaypoints(gen_waypoints_straight_x(car_a_loc, start_y))
self.car_b._setWaypoints(gen_waypoints_straight_x(car_a_loc, start_y))
self.car_a._setDrivingBehavior(AgentDrivingBehavior.FOLLOW_WAYPOINTS)
self.car_a.waypointFollowSpeed = 10
self.car_b._setDrivingBehavior(AgentDrivingBehavior.MAINTAIN_DISTANCE)
self.car_b.followAxis = Rotation2d(1, 0)
self.car_b.followTarget = AgentRepresentation.fromAgent(self.car_a)
self.car_b.followDistance = 3
t_last = time()
while True:
n_tick += 1
self.world.tick()
MeshNode.call(self.car_a.portNumber,
Request('tick', args=[], longRunning=True))
MeshNode.call(self.car_b.portNumber,
Request('tick', args=[], longRunning=True))
car_a_loc = self.car_a.vehiclePose
self.car_a._setWaypoints(
gen_waypoints_straight_x(car_a_loc, start_y))
self.car_b._setWaypoints(
gen_waypoints_straight_x(car_a_loc, start_y))
self.place_spectator(x=car_a_loc.x, y=car_a_loc.y)
dt = time() - t_last
t_last = time()
if dt < enforce_dt:
sleep(enforce_dt - dt)
def test_pathing():
N = 3
objs = []
coordDict = {}
for i in range(N):
a = Agent(ssid=f'AGENT-{str(i)}')
objs.append(a)
(graph, traversed) = MeshNode.call(
objs[0].portNumber,
Request('get_graph_recursive', args=[[]], longRunning=True)).response
# route = objs[0]._routeTo(objs[0].found_agents[3])
# print(objs[0].found_agents[3], route)
targetAgent = MeshNode.call(objs[0].portNumber,
Request('ssid_lookup',
args=['AGENT-1'])).response
targetAgentCoords = MeshNode.call(
objs[0].portNumber,
Request('intra_graph_call', args=[Request('get_pose'),
targetAgent])).response
print(targetAgent, targetAgentCoords)
print(objs[0].negotiatePriority(targetAgent))
def _willCollide(self, futureLitigator: AgentRepresentation):
minDist: float = 3
otherProfile = MeshNode.call(futureLitigator.port,
Request('get_simulation',
args=[5])).response
log = self._getSimulation(5.0)
closest = 1000
for i in range(len(log)):
try:
p_us: Pose2d = log[i][1]
p_them: Pose2d = otherProfile[i][1]
if (p_us.translation - p_them.translation).l2 < closest:
closest = (p_us.translation - p_them.translation).l2
except IndexError:
break
if (closest < minDist):
if self.negotiatePriority(futureLitigator) == futureLitigator:
print(
f"Collision detected between {self.ssid} and {futureLitigator.ssid}! Min spacing of {closest}"
)
return closest < minDist
def spawn_vehicles(self):
self.car_a = Agent(ssid='car_a')
self.car_b = Agent(ssid='car_b')
self.car_a.connect_carla()
self.car_b.connect_carla()
# set up mesh network
MeshNode.call(
self.car_a.portNumber,
Request('get_graph_recursive', args=[[]], longRunning=True))
self.car_a.spawn_vehicle(x=45,
y=-36.5,
z=0.25,
yaw=180,
template_filter='*charger*')
self.car_b.spawn_vehicle(x=35,
y=-10,
z=0.25,
yaw=-90,
template_filter='*cola*')
self.car_a.overridespeed = True
def _solveForProfileToAvoidCollision(self,
futureLitigator: AgentRepresentation):
solved = False
minDist: float = 3 # meter
otherProfile = MeshNode.call(futureLitigator.port,
Request("get_simulation")).response
while not solved:
log = self._getSimulation(5.0)
closest = 1000
for i in range(len(log)):
p_us: Pose2d = log[i][1]
p_them: Pose2d = otherProfile[i][1]
if (p_us.translation - p_them.translation).l2 < closest:
closest = (p_us.translation - p_them.translation).l2
if closest < minDist:
speed = self.velocityReference
if speed == 0.0:
raise Exception("Collision unavoidable!")
self.velocityReference = max(self.velocityReference - 0.2, 0.0)
print(
f"Reducing speed {self.velocityReference + 0.2}->{self.velocityReference}"
)
else:
solved = True
def run(self):
self.world.tick()
n_tick = 0
# self.car_a.velocityReference =8.9408 * 2
start_y = self.car_a.carla_vehicle.get_location().y
start_y_2 = self.car_c.carla_vehicle.get_location().y
lane_width = 4 # m
lane_change_dist = 10 # m
lane_change_x = None
t_last = time()
while True:
n_tick += 1
self.world.tick()
MeshNode.call(self.car_a.portNumber,
Request('tick', args=[], longRunning=True))
MeshNode.call(self.car_b.portNumber,
Request('tick', args=[], longRunning=True))
MeshNode.call(self.car_c.portNumber,
Request('tick', args=[], longRunning=True))
car_a_loc = self.car_a.vehiclePose
car_b_loc = self.car_b.vehiclePose
car_c_loc = self.car_c.vehiclePose
if n_tick == 100:
self.car_a._setDrivingBehavior(
AgentDrivingBehavior.FOLLOW_WAYPOINTS)
self.car_b._setDrivingBehavior(
AgentDrivingBehavior.MAINTAIN_DISTANCE)
self.car_b.followAxis = Rotation2d(1, 0)
self.car_b.followTarget = AgentRepresentation.fromAgent(
self.car_a)
self.car_b.followDistance = 8
self.car_c._setDrivingBehavior(
AgentDrivingBehavior.MAINTAIN_DISTANCE)
self.car_c.followAxis = Rotation2d(1, 0)
self.car_c.followTarget = AgentRepresentation.fromAgent(
self.car_a)
self.car_c.followDistance = 8
self.car_a._setWaypoints(
gen_waypoints_straight_x(car_a_loc, start_y))
self.car_b._setWaypoints(
gen_waypoints_straight_x(car_b_loc, start_y))
if n_tick < 500:
self.car_c._setWaypoints(
gen_waypoints_straight_x(car_c_loc, start_y_2))
else:
self.car_c._setWaypoints(
gen_waypoints_straight_x(car_c_loc, start_y))
if n_tick == 500:
print("Enable merge")
#self.car_b._setDrivingBehavior(AgentDrivingBehavior.MERGING)
self.car_c._setMerge(AgentRepresentation.fromAgent(self.car_a),
AgentRepresentation.fromAgent(self.car_b))
self.place_spectator(x=car_a_loc.x, y=car_a_loc.y)
dt = time() - t_last
t_last = time()
if dt < enforce_dt:
sleep(enforce_dt - dt)
def verify(self) -> bool:
return MeshNode.call(self.port,
Request(function='ssid')).response != self.ssid
def execGraphRequest(self, req: Request, endpoint: AgentRepresentation):
path = self._routeTo(endpoint)
req = Request("route_intra_graph_call", [req, path])
return self.dispatch(req).response
def _followTargetVelocity(self) -> float:
return MeshNode.call(self.followTarget.port,
Request('get_fwd_velocity')).response
def run(self):
self.world.tick()
n_tick = 0
last_tick_t = time()
self.car_1.waypointList = c1waypoints
self.car_2.waypointList = c2waypoints
self.car_3.waypointList = c3waypoints
self.car_4.waypointList = c4waypoints
self.car_5.waypointList = c5waypoints
self.car_1.drivingBehavior = AgentDrivingBehavior.FOLLOW_WAYPOINTS
self.car_2.drivingBehavior = AgentDrivingBehavior.FOLLOW_WAYPOINTS
self.car_3.drivingBehavior = AgentDrivingBehavior.FOLLOW_WAYPOINTS
self.car_4.drivingBehavior = AgentDrivingBehavior.FOLLOW_WAYPOINTS
self.car_5.drivingBehavior = AgentDrivingBehavior.FOLLOW_WAYPOINTS
self.car_1.collisionDetection = False
self.car_2.collisionDetection = False
self.car_3.collisionDetection = False
self.car_4.collisionDetection = False
self.car_5.collisionDetection = False
while True:
n_tick += 1
self.world.tick()
MeshNode.call(self.car_1.portNumber,
Request('tick', args=[], longRunning=True))
MeshNode.call(self.car_2.portNumber,
Request('tick', args=[], longRunning=True))
MeshNode.call(self.car_3.portNumber,
Request('tick', args=[], longRunning=True))
MeshNode.call(self.car_4.portNumber,
Request('tick', args=[], longRunning=True))
MeshNode.call(self.car_5.portNumber,
Request('tick', args=[], longRunning=True))
def t2l(trns):
return carla.Location(trns.x, trns.y, 2)
dbg = self.carla_client.get_world().debug
c1p = self.car_1.vehiclePose.translation
c2p = self.car_2.vehiclePose.translation
c3p = self.car_3.vehiclePose.translation
c4p = self.car_4.vehiclePose.translation
c5p = self.car_5.vehiclePose.translation
dbg.draw_line(t2l(c1p), t2l(c2p), 0.1, carla.Color(255, 0, 0),
0.01)
dbg.draw_line(t2l(c1p), t2l(c3p), 0.1, carla.Color(255, 0, 0),
0.01)
dbg.draw_line(t2l(c3p), t2l(c4p), 0.1, carla.Color(255, 0, 0),
0.01)
dbg.draw_line(t2l(c4p), t2l(c5p), 0.1, carla.Color(255, 0, 0),
0.01)
dbg.draw_line(t2l(c5p), t2l(c3p), 0.1, carla.Color(255, 0, 0),
0.01)
dbg.draw_line(t2l(c4p), t2l(c2p), 0.1, carla.Color(255, 0, 0),
0.01)
dbg.draw_point(t2l(c1p), 0.07, carla.Color(0, 0, 255), 0.01)
dbg.draw_point(t2l(c2p), 0.07, carla.Color(0, 0, 255), 0.01)
dbg.draw_point(t2l(c3p), 0.07, carla.Color(0, 0, 255), 0.01)
dbg.draw_point(t2l(c4p), 0.07, carla.Color(0, 0, 255), 0.01)
dbg.draw_point(t2l(c5p), 0.07, carla.Color(0, 0, 255), 0.01)
# dbg.
low = 30
high = 10
if n_tick < 90:
self.car_1.waypointFollowSpeed = low
else:
self.car_1.waypointFollowSpeed = high
if n_tick < 20:
self.car_2.waypointFollowSpeed = low
else:
self.car_2.waypointFollowSpeed = high
if n_tick < 90:
self.car_3.waypointFollowSpeed = low
else:
self.car_3.waypointFollowSpeed = high
if n_tick < 300:
self.car_4.waypointFollowSpeed = low / 2
else:
self.car_4.waypointFollowSpeed = high
if n_tick < 350:
self.car_5.waypointFollowSpeed = low / 4
else:
self.car_5.waypointFollowSpeed = high
print(f"Tick: {n_tick}")
if n_tick == 1000:
break
print(f"dt: {time() - last_tick_t}")
dt = time() - last_tick_t
if dt < override_dt:
pass
sleep(override_dt - dt)
last_tick_t = time()
def getGraph_Recursive(self, traversedAgents: List[AgentRepresentation]):
print(f"gg_r called on {self.ssid}")
if len(self.found_agents) == 0:
self.found_agents = self.findAgents()
agents = [
agent for agent in self._semiShuffle(self.found_agents)
if agent not in traversedAgents
]
# agents = [agent for agent in agents if len(self.graph.neighbors(agent)) < MAX_CONNECTED_AGENTS]
print(f"untraversed agents: {[i.ssid for i in agents]}")
if AgentRepresentation.fromAgent(self) not in traversedAgents:
traversedAgents.append(AgentRepresentation.fromAgent(self))
if len(agents) > 0:
for agent in agents[0:min(len(agents), MAX_CONNECTED_AGENTS)]:
otherrep = agent
selfrep = AgentRepresentation.fromAgent(self)
if MeshNode.call(otherrep.port, Request(
'num_connections')).response >= MAX_CONNECTED_AGENTS:
continue
(otherGraph, additionallyTraversed) = MeshNode.call(
otherrep.port,
Request('get_graph_recursive',
args=[traversedAgents],
longRunning=True)).response
traversedAgents.extend(additionallyTraversed)
traversedAgents = list(set(traversedAgents))
newGraph = nx.compose(otherGraph, self.graph)
self.graph = newGraph.copy()
# print(f"New graph: {otherGraph}")
# self.graph.add_nodes_from(otherGraph)
# self.graph.add_edges_from(otherGraph)
self.graph.add_edge(selfrep, otherrep)
self.directly_connected.append(otherrep)
print(
f"Number of connections: {len(list(self.graph.neighbors(AgentRepresentation.fromAgent(self))))}"
)
if len(list(self.graph.neighbors(
AgentRepresentation.fromAgent(self)))) < MAX_CONNECTED_AGENTS:
# If there are no unconnected agents, let's add the closest ones that
# A. we don't have any connections with
# B. aren't at max connections
nodes = list(self.graph.nodes())
nodes.remove(AgentRepresentation.fromAgent(self))
agents = []
for agent in nodes:
if len(list(
self.graph.neighbors(agent))) < MAX_CONNECTED_AGENTS:
if not self.graph.has_edge(
AgentRepresentation.fromAgent(self), agent):
agents.append(agent)
agents2 = [
a for a in self._semiShuffle(self.found_agents) if a in agents
]
agents = agents2
for agent in agents[0:min(
MAX_CONNECTED_AGENTS - len(
list(
self.graph.
neighbors(AgentRepresentation.fromAgent(self)))
), len(agents))]:
print(
f"Forming additional connection between {self.ssid} and {agent.ssid}"
)
self.graph.add_edge(agent, AgentRepresentation.fromAgent(self))
self.directly_connected.append(agent)
# print(f"Returning traversed: {traversedAgents}")
assert [
len(list(self.graph.neighbors(i))) <= MAX_CONNECTED_AGENTS
for i in self.graph.nodes()
]
return (self.graph, traversedAgents)
