Skip to content

Commit 303ac3d

Browse files
kennyseyboldkennyseybold
committed
updated piSideCode
1 parent 7a074e3 commit 303ac3d

File tree

4 files changed

+120
-63
lines changed

4 files changed

+120
-63
lines changed

‎PiSideCode/Processing/Processing.py‎

Lines changed: 60 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -68,8 +68,8 @@ def makePoseObject(temp):
6868
temp["rotation"]["quaternion"]["Y"],
6969
temp["rotation"]["quaternion"]["Z"],
7070
)
71-
).rotateBy(wpi.Rotation3d(0, 0, math.radians(180))))
72-
71+
).rotateBy(wpi.Rotation3d(0, 0, math.radians(180))),
72+
)
7373

7474
# ROT MUST BE A 3x3 ROTATION MATRIX
7575
def getTransform(trans, rot):
@@ -79,7 +79,7 @@ def getTransform(trans, rot):
7979

8080
def getTagTransform(jsonID):
8181
pose = Processor.makePoseObject(jsonID)
82-
rot = pose.rotation().rotateBy(wpi.Rotation3d(0,0,math.radians(180)))
82+
rot = pose.rotation().rotateBy(wpi.Rotation3d(0, 0, math.radians(180)))
8383
translation = np.asarray([pose.X(), pose.Y(), pose.Z()]).reshape(3, 1)
8484
rot, _ = cv2.Rodrigues(np.asarray([rot.X(), rot.Y(), rot.Z()]).reshape(3, 1))
8585
return Processor.getTransform(translation, rot)
@@ -97,6 +97,37 @@ def addCorners(tagPos, cornerPos):
9797
tagPos.translation() + cornerPos.rotateBy(tagPos.rotation())
9898
)
9999

100+
@staticmethod
101+
def getTransRots(tvecs, rvecs, curId, layout):
102+
103+
camTvecs = tvecs
104+
105+
# ambig.append(reproj[0][0])
106+
107+
x = camTvecs[0]
108+
y = camTvecs[1]
109+
z = camTvecs[2]
110+
111+
camTvecs = np.asarray([z, -x, -y]).reshape(3, 1)
112+
113+
camRvecs = rvecs
114+
115+
x = camRvecs[0]
116+
y = camRvecs[1]
117+
z = camRvecs[2]
118+
119+
camRvecs = np.asarray([z, -x, -y]).reshape(3, 1)
120+
121+
camRotMat, _ = cv2.Rodrigues(camRvecs)
122+
cam2Tag = Processor.getTransform(camTvecs, camRotMat)
123+
world2Tag = Processor.getTagTransform(layout[curId - 1]["pose"])
124+
125+
world2Cam = np.dot(world2Tag, np.linalg.inv(cam2Tag))
126+
127+
transVec = Processor.getTranslationFromTransform(world2Cam)
128+
rvecs = Processor.getRotFromTransform(world2Cam)
129+
return (transVec, rvecs)
130+
100131
# Find AprilTags and calculate the camera's pose
101132
def imagePose(self, images, K, D, layout, arucoDetector):
102133
poses = []
@@ -105,11 +136,13 @@ def imagePose(self, images, K, D, layout, arucoDetector):
105136

106137
# Loop through images
107138
for imgIndex, img in enumerate(images):
139+
pose1 = 0
140+
pose2 = 0
108141
curTags = []
109142

110143
# If an invalid image is given or no calibration return an error pose
111144
if img is None or K[imgIndex] is None or D[imgIndex] is None:
112-
poses.append(Processor.BAD_POSE)
145+
poses.append((Processor.BAD_POSE, Processor.BAD_POSE))
113146
tags.append([])
114147
ambig.append(2767)
115148
continue
@@ -230,7 +263,7 @@ def imagePose(self, images, K, D, layout, arucoDetector):
230263
if len(ids) > 1:
231264
ambig.append(2767)
232265

233-
if len(ids) == 0:
266+
if len(ids) == 1:
234267
tempc1 = np.asarray(
235268
[self.squareLength / 2, self.squareLength / 2, 0]
236269
)
@@ -251,32 +284,19 @@ def imagePose(self, images, K, D, layout, arucoDetector):
251284
D[imgIndex],
252285
flags=cv2.SOLVEPNP_IPPE_SQUARE,
253286
)
254-
# Processor.logger.info(tvecs)
255-
curId = ids[0][0]
256-
camTvecs = tvecs[0].reshape(3, 1)
257-
258-
x = camTvecs[0]
259-
y = camTvecs[1]
260-
z = camTvecs[2]
261-
262-
camTvecs = np.asarray([z, -x, -y]).reshape(3, 1)
287+
t1, r1 = Processor.getTransRots(tvecs[0].reshape(3,1), rvecs[0], ids[0][0], layout)
288+
t2, r2 = Processor.getTransRots(tvecs[1].reshape(3,1), rvecs[1], ids[0][0], layout)
263289

264-
camRvecs = rvecs[0]
290+
rot3D = wpi.Rotation3d(r1)
291+
trans = wpi.Translation3d(t1[0], t1[1], t1[2])
265292

266-
x = camRvecs[0]
267-
y = camRvecs[1]
268-
z = camRvecs[2]
293+
pose1 = wpi.Pose3d(trans, rot3D)
269294

270-
camRvecs = np.asarray([z, -x, -y]).reshape(3, 1)
295+
rot3D = wpi.Rotation3d(r2)
296+
trans = wpi.Translation3d(t2[0], t2[1], t2[2])
271297

272-
camRotMat, _ = cv2.Rodrigues(camRvecs)
273-
cam2Tag = Processor.getTransform(camTvecs, camRotMat)
274-
world2Tag = Processor.getTagTransform(layout[curId - 1]["pose"])
298+
pose2 = wpi.Pose3d(trans, rot3D)
275299

276-
world2Cam = np.dot(world2Tag, np.linalg.inv(cam2Tag))
277-
278-
transVec = Processor.getTranslationFromTransform(world2Cam)
279-
rvecs = Processor.getRotFromTransform(world2Cam)
280300

281301
else:
282302
# Calculate robot pose with 2d and 3d points
@@ -288,30 +308,33 @@ def imagePose(self, images, K, D, layout, arucoDetector):
288308
flags=cv2.SOLVEPNP_SQPNP,
289309
)
290310

311+
# ambig.append(reproj[0])
312+
291313
# Grab the rotation matrix and find the translation vector
292314
rotMat, _ = cv2.Rodrigues(rvecs)
293315
transVec = -np.dot(np.transpose(rotMat), tvecs)
294316
transVec = np.asarray([transVec[2], -transVec[0], -transVec[1]])
295317

296-
# Convert the rotation matrix to a three rotation system (yaw, pitch, roll)
297-
rot3D = wpi.Rotation3d(
298-
np.array([rvecs[2][0], -rvecs[0][0], +rvecs[1][0]]),
299-
(rvecs[0][0] ** 2 + rvecs[1][0] ** 2 + rvecs[2][0] ** 2) ** 0.5,
300-
)
318+
rots, _ = cv2.Rodrigues(rotMat)
319+
rotMat, _ = cv2.Rodrigues(np.asarray([rots[2], -rots[0], rots[1]]))
301320

321+
# Convert the rotation matrix to a three rotation system (yaw, pitch, roll)
322+
rot3D = wpi.Rotation3d(rotMat)
323+
pose1 = wpi.Pose3d(
324+
# Translation between openCV and WPILib
325+
wpi.Translation3d(transVec[0], transVec[1], transVec[2]),
326+
rot3D,
327+
)
328+
pose2 = pose1
302329
# Append the pose
303330
poses.append(
304-
wpi.Pose3d(
305-
# Translation between openCV and WPILib
306-
wpi.Translation3d(transVec[0], transVec[1], transVec[2]),
307-
rot3D,
308-
)
331+
(pose1, pose2)
309332
)
310333
tags.append(curTags)
311334
num += 1
312335
else:
313336
# If no tags
314-
poses.append(Processor.BAD_POSE)
337+
poses.append((Processor.BAD_POSE, Processor.BAD_POSE))
315338
tags.append([])
316339
ambig.append(2767)
317340

‎PiSideCode/Publisher/NetworkTablePublisher.py‎

Lines changed: 53 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@ class NetworkIO:
66
logger = logging.getLogger(__name__)
77

88
# Create a Network Tables Client with given info
9-
def __init__(self, test, team, tableName):
9+
def __init__(self, test, team, tableName, numCams):
1010
# Grab the default network table instance and grab the table name
1111
self.inst = ntcore.NetworkTableInstance.getDefault()
1212
self.table = self.inst.getTable(tableName)
@@ -15,6 +15,11 @@ def __init__(self, test, team, tableName):
1515
self.inst.startClient4("WallEye_Client")
1616
self.updateNum = []
1717
self.connection = []
18+
self.pose1Sub = []
19+
self.pose2Sub = []
20+
self.timestampSub = []
21+
self.ambiguitySub = []
22+
self.tagSub = []
1823
if test:
1924
self.inst.setServer("127.0.0.1", 5810)
2025
else:
@@ -30,22 +35,51 @@ def __init__(self, test, team, tableName):
3035
self.publishConnection = []
3136

3237
# Pose publisher
33-
for index in range(5):
34-
self.publishers.append(
35-
self.table.getDoubleArrayTopic("Result" + str(index)).publish(
38+
for index in range(numCams):
39+
self.publishers.append(self.table.getSubTable("Result" + str(index)))
40+
41+
self.pose1Sub.append(self.publishers[index].getDoubleArrayTopic("Pose1").publish(
42+
ntcore.PubSubOptions(
43+
periodic=0.01, sendAll=True, keepDuplicates=True
44+
)
45+
)
46+
)
47+
48+
self.timestampSub.append(self.publishers[index].getDoubleTopic("timestamp").publish(
49+
ntcore.PubSubOptions(
50+
periodic=0.01, sendAll=True, keepDuplicates=True
51+
)
52+
)
53+
)
54+
55+
self.pose2Sub.append(self.publishers[index].getDoubleArrayTopic("Pose2").publish(
56+
ntcore.PubSubOptions(
57+
periodic=0.01, sendAll=True, keepDuplicates=True
58+
)
59+
)
60+
)
61+
62+
self.ambiguitySub.append(self.publishers[index].getDoubleTopic("ambiguity").publish(
3663
ntcore.PubSubOptions(
3764
periodic=0.01, sendAll=True, keepDuplicates=True
3865
)
3966
)
4067
)
68+
69+
self.tagSub.append(self.publishers[index].getIntegerArrayTopic("tags").publish(
70+
ntcore.PubSubOptions(
71+
periodic=0.01, sendAll=True, keepDuplicates=True
72+
)
73+
)
74+
)
75+
4176
self.publishUpdate.append(
4277
self.table.getIntegerTopic("Update" + str(index)).publish(
4378
ntcore.PubSubOptions(
4479
periodic=0.01, sendAll=True, keepDuplicates=True
4580
)
4681
)
4782
)
48-
4983
self.publishConnection.append(
5084
self.table.getBooleanTopic("Connected" + str(index)).publish(
5185
ntcore.PubSubOptions(
@@ -65,22 +99,20 @@ def setTable(self, name):
6599

66100
# Publishes the supplied pose information in the corresponding publisher
67101
def publish(self, index, time, pose, tags, ambig):
68-
t = pose.translation()
69-
r = pose.rotation()
70-
result = [
71-
t.X(),
72-
t.Y(),
73-
t.Z(),
74-
r.X(),
75-
r.Y(),
76-
r.Z(),
77-
ntcore._now() - float(time),
78-
len(tags),
79-
]
80-
for i in range(len(tags)):
81-
result.append(tags[i])
82-
result.append(ambig)
83-
self.publishers[index].set(result)
102+
pose1 = pose[0]
103+
pose2 = pose[1]
104+
t1 = pose1.translation()
105+
r1 = pose1.rotation()
106+
t2 = pose2.translation()
107+
r2 = pose2.rotation()
108+
109+
self.pose1Sub[index].set([t1.X(), t1.Y(), t1.Z(), r1.X(), r1.Y(), r1.Z()])
110+
self.pose2Sub[index].set([t2.X(), t2.Y(), t2.Z(), r2.X(), r2.Y(), r2.Z()])
111+
self.ambiguitySub[index].set(ambig)
112+
self.tagSub[index].set(tags)
113+
self.timestampSub[index].set(ntcore._now() - time)
114+
115+
84116
self.updateNum[index] += 1
85117
self.publishUpdate[index].set(self.updateNum[index])
86118

‎PiSideCode/init.py‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -172,9 +172,11 @@
172172
elif walleyeData.currentState == States.PROCESSING:
173173
# Set tag size, grab camera frames, and grab image timestamp
174174
poseEstimator.setTagSize(walleyeData.tagSize)
175+
175176
imageTime = walleyeData.robotPublisher.getTime()
177+
176178
connections, images = walleyeData.cameras.getFramesForProcessing()
177-
179+
178180
for idx, val in enumerate(connections.values()):
179181
if not val and walleyeData.robotPublisher.getConnectionValue(idx):
180182
logger.info("Camera disconnected")
@@ -191,7 +193,7 @@
191193
# logger.info(f"Poses at {imageTime}: {poses}")
192194

193195
for i in range(len(poses)):
194-
if poses[i].X() < 2000:
196+
if poses[i][0].X() < 2000:
195197
walleyeData.robotPublisher.publish(
196198
i, imageTime, poses[i], tags[i], ambig[i]
197199
)
@@ -201,10 +203,10 @@
201203
if i >= len(poses):
202204
break
203205
camBuffers[identifier].update(img)
204-
walleyeData.setPose(identifier, poses[i])
206+
walleyeData.setPose(identifier, poses[i][0])
205207
if walleyeData.visualizingPoses:
206208
visualizationBuffers[identifier].update(
207-
(poses[i].X(), poses[i].Y(), poses[i].Z()), tags[i][1:]
209+
(poses[i][0].X(), poses[i][0].Y(), poses[i][0].Z()), tags[i][1:]
208210
)
209211

210212
# Ends the WallEye program through the web interface

‎PiSideCode/state.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -109,7 +109,7 @@ def makePublisher(self, teamNumber, tableName):
109109
Config.logger.info("Existing publisher destroyed")
110110

111111
# Create the robot publisher
112-
self.robotPublisher = NetworkIO(False, self.teamNumber, self.tableName)
112+
self.robotPublisher = NetworkIO(False, self.teamNumber, self.tableName, 2)
113113

114114
Config.logger.info(f"Robot publisher created: {teamNumber} - {tableName}")
115115

0 commit comments

Comments
 (0)