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 predict(self,
velocity_profile: Callable,
waypoints: List[Translation2d],
pose0: Pose2d,
predict_dt: float = 0.01,
predict_end: float = 5.0):
"""
Predict the car's behavior for the coming time
"""
# interpolate pure pursuit at predict_dt frequency
pose = pose0
pts = waypoints.copy()
margin = 0.1
log = []
for t in np.linspace(0,
predict_end,
int(predict_end / predict_dt),
endpoint=False):
if (pose.translation - waypoints[0]).l2 <= margin:
print("Dropping first waypoint")
waypoints.pop(0)
if len(waypoints) == 0:
break
vel = velocity_profile(t)
wp = waypoints[0]
wp_dp = wp - pose.translation
steering_angle = self.compute_steering_ik(wp_dp, pose.rotation)
twist = self.compute_fk(steering_angle, vel, dt=predict_dt)
#pose = pose.exp(twist)
pose.rotation = pose.rotation.rotateBy(
Rotation2d.createFromRadians(twist.dtheta))
pose.translation = pose.translation.translateBy(
Translation2d(twist.dx,
twist.dy).rotateByOrigin(pose.rotation))
log.append([
t,
Pose2d(trans=Translation2d(pose.translation.x,
pose.translation.y),
rot=Rotation2d(pose.rotation.cos, pose.rotation.sin))
])
return log
def compute_steering_ik(self, target: Translation2d,
current_heading: Rotation2d):
"""
Computes the steering angle to hit the desired target
:param target_x: X coordinate in m, relative to the car's origin
:param target_y: Y coordinate in m, relative to the car's origin
:param current_heading: Current heading, in rads
:return: Steering angle, in rads
"""
current_heading = current_heading.inverse
relative = target.rotateByOrigin(current_heading)
if relative.y == 0:
# Protect against divide by 0
steering_angle = 0
elif relative.x == 0:
# Should never happen, if it does, we steer as far as we can
steering_angle = self.max_steering_angle * relative.y / abs(
relative.y)
else:
turning_radius = (relative.x**2 + relative.y**2) / (
2 * relative.y) # Turning radius in m
steering_angle = np.arctan2(self.wheelbase_length, turning_radius)
if steering_angle > np.pi / 2:
steering_angle -= (np.pi)
# threshold steering angle to our limits
steering_angle = min(max(steering_angle, -self.max_steering_angle),
self.max_steering_angle)
return steering_angle
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()
class Pose2d:
translation = Translation2d()
rotation = Rotation2d()
def __init__(self, trans=Translation2d(), rot=Rotation2d()):
self.translation = trans
self.rotation = rot
@property
def x(self):
return self.translation.x
@property
def y(self):
return self.translation.y
@property
def cos(self):
return self.rotation.cos
@property
def sin(self):
return self.rotation.sin
@property
def theta(self):
return self.rotation.radians
def transformBy(self, other):
return Pose2d(self.translation.translateBy(other.translation),
self.rotation.rotateBy(other.rotation))
def relativeTransformBy(self, other):
return Pose2d(
self.translation.translateBy(
other.translation.rotateByOrigin(self.rotation)),
self.rotation.rotateBy(other.rotation))
@property
def inverse(self):
return Pose2d(
self.translation.inverse().rotateByOrigin(self.rotation.inverse),
self.rotation.inverse)
@staticmethod
def exp(delta: Twist2d):
sin_theta = math.sin(delta.dtheta)
cos_theta = math.cos(delta.dtheta)
if (math.fabs(delta.dtheta) < 1E-6):
s = 1.0 - 1. / 6. * delta.dtheta * delta.dtheta
c = 0.5 * delta.dtheta
else:
s = sin_theta / delta.dtheta
c = (1.0 - cos_theta) / delta.dtheta
return Pose2d(
Translation2d(delta.dx * s - delta.dy * c,
delta.dx * c + delta.dy * s),
Rotation2d(cos_theta, sin_theta))
def gen_waypoints_straight_x(location,
original_y,
init_dist=10,
dist=1,
num_points=25):
waypoints = []
x = location.x + init_dist
for i in range(num_points):
waypoints.append(Translation2d(x + dist, original_y))
x += dist
return waypoints
def gen_waypoints_straight_y(location,
original_x,
init_dist=-10,
dist=-1,
num_points=25):
waypoints = []
y = location.y + init_dist
for i in range(num_points):
waypoints.append(Translation2d(original_x, y))
y += dist
return waypoints
def gen_lanechange_waypoints(location,
original_y,
final_y,
change_x,
init_dist=10,
dist=1,
num_points=25):
waypoints = []
x = location.x + init_dist
for i in range(num_points):
x += dist
y = original_y if x < change_x else final_y
waypoints.append(Translation2d(x, y))
return waypoints
def exp(delta: Twist2d):
sin_theta = math.sin(delta.dtheta)
cos_theta = math.cos(delta.dtheta)
if (math.fabs(delta.dtheta) < 1E-6):
s = 1.0 - 1. / 6. * delta.dtheta * delta.dtheta
c = 0.5 * delta.dtheta
else:
s = sin_theta / delta.dtheta
c = (1.0 - cos_theta) / delta.dtheta
return Pose2d(
Translation2d(delta.dx * s - delta.dy * c,
delta.dx * c + delta.dy * s),
Rotation2d(cos_theta, sin_theta))
def hardcoded_cop_waypoints():
return [
Translation2d(35, -36),
Translation2d(27.5, -20),
Translation2d(27.5, -15),
Translation2d(27.5, -5),
Translation2d(27.5, 5),
Translation2d(30, 15),
]
def __init__(self, trans=Translation2d(), rot=Rotation2d()):
self.translation = trans
self.rotation = rot
def hardcoded_cop_waypoints():
return [
Translation2d(35, -36),
Translation2d(30, -36),
Translation2d(30, -25),
]
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()
c1waypoints = [
Translation2d(-46.8, 17.0),
Translation2d(-46.8, -40),
Translation2d(-46.8, -70),
Translation2d(-46.8, -100),
]
c2waypoints = [
Translation2d(-33.65, -1),
Translation2d(-63.65, -1),
Translation2d(-83.65, -1),
Translation2d(-100, -1),
]
c3waypoints = [
Translation2d(-33.65, -4.10),
Translation2d(-40.65, -4.10),
Translation2d(-46.8, -10),
Translation2d(-46.8, -15),
