Merge pull request #15 from strykeforce/newPiSideCode

updated piSideCode

Author: kennyseybold

PiSideCode/Processing/Processing.py

@@ -68,8 +68,8 @@ def makePoseObject(temp):
                     temp["rotation"]["quaternion"]["Y"],
                     temp["rotation"]["quaternion"]["Z"],
                 )
-            ).rotateBy(wpi.Rotation3d(0, 0, math.radians(180))))
-        
+            ).rotateBy(wpi.Rotation3d(0, 0, math.radians(180))),
+        )
 
     # ROT MUST BE A 3x3 ROTATION MATRIX
     def getTransform(trans, rot):
@@ -79,7 +79,7 @@ def getTransform(trans, rot):
 
     def getTagTransform(jsonID):
         pose = Processor.makePoseObject(jsonID)
-        rot = pose.rotation().rotateBy(wpi.Rotation3d(0,0,math.radians(180)))
+        rot = pose.rotation().rotateBy(wpi.Rotation3d(0, 0, math.radians(180)))
         translation = np.asarray([pose.X(), pose.Y(), pose.Z()]).reshape(3, 1)
         rot, _ = cv2.Rodrigues(np.asarray([rot.X(), rot.Y(), rot.Z()]).reshape(3, 1))
         return Processor.getTransform(translation, rot)
@@ -97,6 +97,37 @@ def addCorners(tagPos, cornerPos):
             tagPos.translation() + cornerPos.rotateBy(tagPos.rotation())
         )
 
+    @staticmethod
+    def getTransRots(tvecs, rvecs, curId, layout):
+
+        camTvecs = tvecs
+
+        # ambig.append(reproj[0][0])
+
+        x = camTvecs[0]
+        y = camTvecs[1]
+        z = camTvecs[2]
+
+        camTvecs = np.asarray([z, -x, -y]).reshape(3, 1)
+
+        camRvecs = rvecs
+
+        x = camRvecs[0]
+        y = camRvecs[1]
+        z = camRvecs[2]
+
+        camRvecs = np.asarray([z, -x, -y]).reshape(3, 1)
+
+        camRotMat, _ = cv2.Rodrigues(camRvecs)
+        cam2Tag = Processor.getTransform(camTvecs, camRotMat)
+        world2Tag = Processor.getTagTransform(layout[curId - 1]["pose"])
+
+        world2Cam = np.dot(world2Tag, np.linalg.inv(cam2Tag))
+
+        transVec = Processor.getTranslationFromTransform(world2Cam)
+        rvecs = Processor.getRotFromTransform(world2Cam)
+        return (transVec, rvecs)
+
     # Find AprilTags and calculate the camera's pose
     def imagePose(self, images, K, D, layout, arucoDetector):
         poses = []
@@ -105,11 +136,13 @@ def imagePose(self, images, K, D, layout, arucoDetector):
 
         # Loop through images
         for imgIndex, img in enumerate(images):
+            pose1 = 0
+            pose2 = 0
             curTags = []
 
             # If an invalid image is given or no calibration return an error pose
             if img is None or K[imgIndex] is None or D[imgIndex] is None:
-                poses.append(Processor.BAD_POSE)
+                poses.append((Processor.BAD_POSE, Processor.BAD_POSE))
                 tags.append([])
                 ambig.append(2767)
                 continue
@@ -230,7 +263,7 @@ def imagePose(self, images, K, D, layout, arucoDetector):
                     if len(ids) > 1:
                         ambig.append(2767)
 
-                if len(ids) == 0:
+                if len(ids) == 1:
                     tempc1 = np.asarray(
                         [self.squareLength / 2, self.squareLength / 2, 0]
                     )
@@ -251,32 +284,19 @@ def imagePose(self, images, K, D, layout, arucoDetector):
                         D[imgIndex],
                         flags=cv2.SOLVEPNP_IPPE_SQUARE,
                     )
-                    # Processor.logger.info(tvecs)
-                    curId = ids[0][0]
-                    camTvecs = tvecs[0].reshape(3, 1)
-
-                    x = camTvecs[0]
-                    y = camTvecs[1]
-                    z = camTvecs[2]
-
-                    camTvecs = np.asarray([z, -x, -y]).reshape(3, 1)
+                    t1, r1 = Processor.getTransRots(tvecs[0].reshape(3,1), rvecs[0], ids[0][0], layout)
+                    t2, r2 = Processor.getTransRots(tvecs[1].reshape(3,1), rvecs[1], ids[0][0], layout)
 
-                    camRvecs = rvecs[0]
+                    rot3D = wpi.Rotation3d(r1)
+                    trans = wpi.Translation3d(t1[0], t1[1], t1[2])
 
-                    x = camRvecs[0]
-                    y = camRvecs[1]
-                    z = camRvecs[2]
+                    pose1 = wpi.Pose3d(trans, rot3D)
 
-                    camRvecs = np.asarray([z, -x, -y]).reshape(3, 1)
+                    rot3D = wpi.Rotation3d(r2)
+                    trans = wpi.Translation3d(t2[0], t2[1], t2[2])
 
-                    camRotMat, _ = cv2.Rodrigues(camRvecs)
-                    cam2Tag = Processor.getTransform(camTvecs, camRotMat)
-                    world2Tag = Processor.getTagTransform(layout[curId - 1]["pose"])
+                    pose2 = wpi.Pose3d(trans, rot3D)
 
-                    world2Cam = np.dot(world2Tag, np.linalg.inv(cam2Tag))
-
-                    transVec = Processor.getTranslationFromTransform(world2Cam)
-                    rvecs = Processor.getRotFromTransform(world2Cam)
 
                 else:
                     # Calculate robot pose with 2d and 3d points
@@ -288,30 +308,33 @@ def imagePose(self, images, K, D, layout, arucoDetector):
                         flags=cv2.SOLVEPNP_SQPNP,
                     )
 
+                    # ambig.append(reproj[0])
+                    
                     # Grab the rotation matrix and find the translation vector
                     rotMat, _ = cv2.Rodrigues(rvecs)
                     transVec = -np.dot(np.transpose(rotMat), tvecs)
                     transVec = np.asarray([transVec[2], -transVec[0], -transVec[1]])
 
-                # Convert the rotation matrix to a three rotation system (yaw, pitch, roll)
-                rot3D = wpi.Rotation3d(
-                    np.array([rvecs[2][0], -rvecs[0][0], +rvecs[1][0]]),
-                    (rvecs[0][0] ** 2 + rvecs[1][0] ** 2 + rvecs[2][0] ** 2) ** 0.5,
-                )
+                    rots, _ = cv2.Rodrigues(rotMat)
+                    rotMat, _ = cv2.Rodrigues(np.asarray([rots[2], -rots[0], rots[1]]))
 
+                    # Convert the rotation matrix to a three rotation system (yaw, pitch, roll)
+                    rot3D = wpi.Rotation3d(rotMat)
+                    pose1 = wpi.Pose3d(
+                            # Translation between openCV and WPILib
+                            wpi.Translation3d(transVec[0], transVec[1], transVec[2]),
+                            rot3D,
+                        )
+                    pose2 = pose1
                 # Append the pose
                 poses.append(
-                    wpi.Pose3d(
-                        # Translation between openCV and WPILib
-                        wpi.Translation3d(transVec[0], transVec[1], transVec[2]),
-                        rot3D,
-                    )
+                    (pose1, pose2)
                 )
                 tags.append(curTags)
                 num += 1
             else:
                 # If no tags
-                poses.append(Processor.BAD_POSE)
+                poses.append((Processor.BAD_POSE, Processor.BAD_POSE))
                 tags.append([])
                 ambig.append(2767)
 

PiSideCode/Publisher/NetworkTablePublisher.py

@@ -6,7 +6,7 @@ class NetworkIO:
     logger = logging.getLogger(__name__)
 
     # Create a Network Tables Client with given info
-    def __init__(self, test, team, tableName):
+    def __init__(self, test, team, tableName, numCams):
         # Grab the default network table instance and grab the table name
         self.inst = ntcore.NetworkTableInstance.getDefault()
         self.table = self.inst.getTable(tableName)
@@ -15,6 +15,11 @@ def __init__(self, test, team, tableName):
         self.inst.startClient4("WallEye_Client")
         self.updateNum = []
         self.connection = []
+        self.pose1Sub = []
+        self.pose2Sub = []
+        self.timestampSub = []
+        self.ambiguitySub = []
+        self.tagSub = []
         if test:
             self.inst.setServer("127.0.0.1", 5810)
         else:
@@ -30,22 +35,51 @@ def __init__(self, test, team, tableName):
         self.publishConnection = []
 
         # Pose publisher
-        for index in range(5):
-            self.publishers.append(
-                self.table.getDoubleArrayTopic("Result" + str(index)).publish(
+        for index in range(numCams):
+            self.publishers.append(self.table.getSubTable("Result" + str(index)))
+
+            self.pose1Sub.append(self.publishers[index].getDoubleArrayTopic("Pose1").publish(
+                    ntcore.PubSubOptions(
+                        periodic=0.01, sendAll=True, keepDuplicates=True
+                    )
+                )
+            )
+
+            self.timestampSub.append(self.publishers[index].getDoubleTopic("timestamp").publish(
+                    ntcore.PubSubOptions(
+                        periodic=0.01, sendAll=True, keepDuplicates=True
+                    )
+                )
+            )
+
+            self.pose2Sub.append(self.publishers[index].getDoubleArrayTopic("Pose2").publish(
+                    ntcore.PubSubOptions(
+                        periodic=0.01, sendAll=True, keepDuplicates=True
+                    )
+                )
+            )
+
+            self.ambiguitySub.append(self.publishers[index].getDoubleTopic("ambiguity").publish(
                     ntcore.PubSubOptions(
                         periodic=0.01, sendAll=True, keepDuplicates=True
                     )
                 )
             )
+
+            self.tagSub.append(self.publishers[index].getIntegerArrayTopic("tags").publish(
+                    ntcore.PubSubOptions(
+                        periodic=0.01, sendAll=True, keepDuplicates=True
+                    )
+                )
+            )
+
             self.publishUpdate.append(
                 self.table.getIntegerTopic("Update" + str(index)).publish(
                     ntcore.PubSubOptions(
                         periodic=0.01, sendAll=True, keepDuplicates=True
                     )
                 )
             )
-
             self.publishConnection.append(
                 self.table.getBooleanTopic("Connected" + str(index)).publish(
                     ntcore.PubSubOptions(
@@ -65,22 +99,20 @@ def setTable(self, name):
 
     # Publishes the supplied pose information in the corresponding publisher
     def publish(self, index, time, pose, tags, ambig):
-        t = pose.translation()
-        r = pose.rotation()
-        result = [
-            t.X(),
-            t.Y(),
-            t.Z(),
-            r.X(),
-            r.Y(),
-            r.Z(),
-            ntcore._now() - float(time),
-            len(tags),
-        ]
-        for i in range(len(tags)):
-            result.append(tags[i])
-        result.append(ambig)
-        self.publishers[index].set(result)
+        pose1 = pose[0]
+        pose2 = pose[1]
+        t1 = pose1.translation()
+        r1 = pose1.rotation()
+        t2 = pose2.translation()
+        r2 = pose2.rotation()
+
+        self.pose1Sub[index].set([t1.X(), t1.Y(), t1.Z(), r1.X(), r1.Y(), r1.Z()])
+        self.pose2Sub[index].set([t2.X(), t2.Y(), t2.Z(), r2.X(), r2.Y(), r2.Z()])
+        self.ambiguitySub[index].set(ambig)
+        self.tagSub[index].set(tags)
+        self.timestampSub[index].set(ntcore._now() - time)
+
+
         self.updateNum[index] += 1
         self.publishUpdate[index].set(self.updateNum[index])
 

PiSideCode/init.py

@@ -172,9 +172,11 @@
         elif walleyeData.currentState == States.PROCESSING:
             # Set tag size, grab camera frames, and grab image timestamp
             poseEstimator.setTagSize(walleyeData.tagSize)
+            
             imageTime = walleyeData.robotPublisher.getTime()
+
             connections, images = walleyeData.cameras.getFramesForProcessing()
-            
+
             for idx, val in enumerate(connections.values()):
                 if not val and walleyeData.robotPublisher.getConnectionValue(idx):
                     logger.info("Camera disconnected")
@@ -191,7 +193,7 @@
             # logger.info(f"Poses at {imageTime}: {poses}")
 
             for i in range(len(poses)):
-                if poses[i].X() < 2000:
+                if poses[i][0].X() < 2000:
                     walleyeData.robotPublisher.publish(
                         i, imageTime, poses[i], tags[i], ambig[i]
                     )
@@ -201,10 +203,10 @@
                 if i >= len(poses):
                     break
                 camBuffers[identifier].update(img)
-                walleyeData.setPose(identifier, poses[i])
+                walleyeData.setPose(identifier, poses[i][0])
                 if walleyeData.visualizingPoses:
                     visualizationBuffers[identifier].update(
-                        (poses[i].X(), poses[i].Y(), poses[i].Z()), tags[i][1:]
+                        (poses[i][0].X(), poses[i][0].Y(), poses[i][0].Z()), tags[i][1:]
                     )
 
         # Ends the WallEye program through the web interface

PiSideCode/state.py

@@ -109,7 +109,7 @@ def makePublisher(self, teamNumber, tableName):
             Config.logger.info("Existing publisher destroyed")
 
         # Create the robot publisher
-        self.robotPublisher = NetworkIO(False, self.teamNumber, self.tableName)
+        self.robotPublisher = NetworkIO(False, self.teamNumber, self.tableName, 2)
 
         Config.logger.info(f"Robot publisher created: {teamNumber} - {tableName}")