class CameraInterface():
    def __init__(self):
        self.name = "camera"

        # Keep track of all targets seen to ensure only one fwdkin is pushed downstream for each
        self.target_log = set()

        # Targets defined here for simulation only
        self.cTurret = CameraTurret([
            Target(np.array([0, 100, 100]).reshape(3, 1), "t0"),
            Target(np.array([0, 100, 150]).reshape(3, 1), "t1"),
            Target(np.array([0, 150, 150]).reshape(3, 1), "t2")
        ])

        self.Comms = Comms()
        self.Comms.add_subscriber_port('127.0.0.1', '3000', 'cState')
        self.Comms.add_publisher_port('127.0.0.1', '3004', 'targetPos')

        # twenty frames per second
        self.frame_delay = .05

    def getCState(self):
        try:
            camera_pos = self.Comms.get('cState').payload
            self.cTurret = CameraTurret([
                Target(np.array([0, 100, 100]).reshape(3, 1), "t0"),
                Target(np.array([0, 100, 150]).reshape(3, 1), "t1"),
                Target(np.array([0, 150, 150]).reshape(3, 1), "t2")
            ], camera_pos[0][0], camera_pos[1][0])
        except queue.Empty:
            pass

    def publishPos(self, id, target):
        """
        Publishes target coordinates and target id in origin frame to pathing algorithms
        """
        self.Comms.define_and_send(id, 'targetPos', target)

    def runS(self):
        prev_frame = time.time()

        while (True):
            # If a time delay has passed, grab a frame (in simulation, do nothing)
            if (time.time() - prev_frame >= self.frame_delay):
                prev_frame = time.time()

            # Update camera turret coords
            self.getCState()

            # run marker detection on frame (or for simulation, check for targets in range)
            targets = self.cTurret.targetsInView()

            # Get new targets and add to log set
            new_targets = {
                key: targets[key]
                for key in (targets.keys() - self.target_log)
            }
            self.target_log = self.target_log | new_targets.keys()

            # if marker detected (or target is within view range for simulation), compute forward kinematics.
            #   for simulation, first derive target in camera frame, then run forward kinematics
            # on target and publish with topic set to marker id.
            R01 = zRot(self.cTurret.q1_given)
            R12 = yRot(self.cTurret.q2_given)
            p_1 = self.cTurret.orig + R01 @ self.cTurret.p12
            p_2 = p_1 + R01 @ R12 @ self.cTurret.pOffset
            t_links = self.cTurret.getTargetLinks(p_2, new_targets.values())

            for (t_link, target) in zip(t_links, new_targets.keys()):
                targ_pos = self.cTurret.representTarget(t_link)
                self.publishPos(target, targ_pos)
                print(targ_pos)

            time.sleep(.02)

    # TODO perform opencv marker detection
    def runR(self):
        # Get marker dictionary set
        marker_dict = aruco.Dictionary_get(aruco.DICT_5X5_1000)

        # Get default detector parameters
        d_params = aruco.DetectorParameters_create()

        # get camera parameters
        cameraMatrix = np.array([])
        distCoeffs = np.array([])
        with open('cameraData.json', 'r') as cameraData:
            data = json.load(cameraData)
            for camera in data['cameras']:
                if camera['name'] == 'Sahith Mac':
                    parameters = camera['parameters']
                    cameraMatrix = np.array([
                        parameters['lw'], 0, parameters['u0'], 0,
                        parameters['lh'], parameters['v0'], 0, 0, 1
                    ]).reshape(3, 3)
                    distCoeffs = np.array([parameters['distortions']])

        # Set up videocap stream
        cap = cv2.VideoCapture(0)

        # time previous frame was captured
        prev_frame = time.time()

        while (True):
            targets = None
            target_ids = None

            # If a time delay has passed, grab a frame (in simulation, do nothing)
            if (time.time() - prev_frame >= self.frame_delay):
                # grab a frame from videocap
                ret, frame = cap.read()

                # Detect markers and corners in frame
                corners, target_ids, rejected = aruco.detectMarkers(
                    frame, marker_dict, parameters=d_params)

                # Estimate marker pose
                _, targets, _ = aruco.estimatePoseSingleMarkers(
                    corners, 0.05, cameraMatrix, distCoeffs)

                prev_frame = time.time()

            # Update camera turret coords
            self.getCState()

            if targets is None:
                continue

            # Get new targets and add to log set
            new_ids = set(target_ids.flatten()) - self.target_log
            self.target_log = self.target_log | set(target_ids.flatten())
            #new_targets = [targets for i in zip(target_ids, targets)]

            # if marker detected, compute forward kinematics.
            for (t_link, id) in zip(targets, target_ids):
                if id not in list(new_ids):
                    continue
                targ_pos = np.array([[-1, 1, 1]]).T * zRot(-np.pi / 2) @ xRot(
                    -np.pi / 2) @ self.cTurret.representTarget(1000 * t_link.T)
                self.publishPos(id, targ_pos)
                print("Targ {}:".format(id))
                print(targ_pos)
                print()

            time.sleep(.05)

    def run(self, is_sim):
        if is_sim:
            self.runS()
        else:
            self.runR()
class HardwareInterface():
    def __init__(self):
        self.name = "hardware"
        self.gun_config = np.array([[0, 0]]).T
        self.camera_config = np.array([[0, 0]]).T
        self.gun_ready = 0

        self.gunReady = True
        self.gunTurretReady = False

        # When a target has been detected and
        self.cameraPath = None
        self.gunPath = None

        # Simulation graphics engine
        self.sim_out = Engine(
            np.array([150, 150, 150]).reshape(3, 1),
            np.array([np.pi / 2 - .4, -1, .3]).reshape(3, 1))

        self.Comms = Comms()
        self.Comms.add_publisher_port('127.0.0.1', '3000', 'cState')
        self.Comms.add_publisher_port('127.0.0.1', '3001', 'gState')
        self.Comms.add_subscriber_port('127.0.0.1', '3002', 'cameraPath')
        self.Comms.add_subscriber_port('127.0.0.1', '3003', 'gunPath')

    def initSerial(self):
        # Establish serial port and baud rate
        self.ard = serial.Serial('COM3', 9600)
        if (self.ard.isOpen() == False):
            self.ard.open()

    def readStateR(self):
        line = []
        while True:
            for c in self.ard.read():
                if chr(c) == ':':
                    # parse return from arduino
                    #print("".join(line))

                    # Run comma delimited parse on [2:]
                    parsed = "".join(line[2:]).split(',')
                    if (line[0] == 'c'):
                        self.camera_config[0][0] = float(parsed[0])
                        self.camera_config[1][0] = float(parsed[1])
                    elif (line[0] == 'g'):
                        self.gun_config[0][0] = float(parsed[0])
                        self.gun_config[1][0] = float(parsed[1])
                    elif (line[0] == 's'):
                        self.gun_ready = float(parsed[0])
                    """
                    print(camera_config)
                    print(gun_config)
                    print(gun_ready)
                    """

                    line = []
                    break
                else:
                    line.append(chr(c))

    def readStateS(self):
        """
        Reads servo configurations and updates internal state (simulation)
        """
        # Do nothing, since ideal situation has interface state always equal to actual hardware state
        self.camera_config = self.camera_config
        self.gun_config = self.gun_config

    def writeCamR(self, ctarg):
        """
        Writes a servo configuration (hardware)
        """
        self.ard.write("c,{},{}:".format(ctarg[0][0], ctarg[1][0]).encode())

    def writeCamS(self, ctarg):
        """
        Writes a servo configuration (simulation)
        """
        # Updates engine with new configurations and draws
        self.sim_out.clearGeometries()

        self.camera_config = ctarg

        # Grid and target dimensions are hardcoded
        self.drawSim(ctarg, self.gun_config)

        #self.sim_out.update()

    def writeGunR(self, gtarg):
        self.ard.write("g,{},{}:".format(gtarg[0][0], gtarg[1][0]).encode())

    def writeGunS(self, gtarg):
        """
        Writes a servo configuration (simulation)
        """
        # Updates engine with new configurations and draws
        self.sim_out.clearGeometries()

        self.gun_config = gtarg

        # Grid and target dimensions are hardcoded
        self.drawSim(self.camera_config, gtarg)

        # self.sim_out.update()

    def writeShootR(self):
        self.ard.write("s:".encode())

    def publishState(self):
        """
        Publishes current configuration states of gun turret and camera turret to both publisher ports
        """
        self.Comms.define_and_send(self.name, 'cState', self.camera_config)
        self.Comms.define_and_send(self.name, 'gState', self.gun_config)

    def receivePath(self):
        """
        Pulls path matrices from subscriber bus and replace current path matrices
        """
        try:
            self.cameraPath = self.Comms.get('cameraPath').payload
        except queue.Empty:
            pass

        try:
            self.gunPath = self.Comms.get('gunPath').payload
        except queue.Empty:
            pass

    def drawSim(self, cameraPos, gunPos):
        self.sim_out.addGeometry([Grid(20, 20)])
        self.sim_out.addGeometry([
            Target(np.array([0, 100, 100]).reshape(3, 1), "t0"),
            Target(np.array([0, 100, 150]).reshape(3, 1), "t1"),
            Target(np.array([0, 150, 150]).reshape(3, 1), "t2")
        ])
        self.sim_out.addGeometry([
            CameraTurret([
                Target(np.array([0, 100, 100]).reshape(3, 1), "t0"),
                Target(np.array([0, 100, 150]).reshape(3, 1), "t1"),
                Target(np.array([0, 150, 150]).reshape(3, 1), "t2")
            ], cameraPos[0, 0], cameraPos[1, 0])
        ])
        self.sim_out.addGeometry(
            [GunTurret(gunPos[0, 0], gunPos[1, 0], 70, [-100, 50, 100])])

    # def fireTheShot(self):
    #     # TODO: Send a fire command to the gun to Arduino and start reloading
    #     self.gunReady = False

    # def checkTurretReady(self):
    #     """
    #     Check if the turret is aiming at the target
    #     """
    #     error = 0.02
    #     if self.gunPath[-1][0] * (1 - error) < self.gun_config[0] < self.gunPath[-1][0] * (1 + error) and \
    #        self.gunPath[-1][1] * (1 - error) < self.gun_config[1] < self.gunPath[-1][1] * (1 + error):
    #         self.gunReady = True
    #     else:
    #         self.gunReady = False

    def runS(self):
        """
        Check subscriber bus for path matrix. If path is found, clear cameraPath and replace with new path.
        If no path is found,
        """
        prev_gun_write = time.time()
        gun_write_delay = 0

        prev_camera_write = time.time()
        camera_write_delay = 0

        self.drawSim(self.camera_config, self.gun_config)

        while (True):
            # Update configuration states and publish them
            self.readStateS()
            self.publishState()
            #self.checkTurretReady()
            # Pull path matrix from queue and replace existing matrices
            self.receivePath()

            # If path steps exist and sufficient time has elapsed since the last write, write again
            if self.gunPath is not None and self.gunPath.shape[1] != 0 and\
               time.time() - prev_gun_write >= gun_write_delay:

                # Write config
                self.writeGunS(self.gunPath[1:3, 0:1])
                prev_gun_write = time.time()

                # Set new delay
                if self.gunPath.shape[1] > 1:
                    # subtract next timestamp from current timestamp for delay
                    gun_write_delay = self.gunPath[0][1] - self.gunPath[0][0]
                else:
                    gun_write_delay = 0

                # Pop current config
                self.gunPath = self.gunPath[0:3, 1:]

            if self.cameraPath is not None and self.cameraPath.shape[1] != 0 and\
               time.time() - prev_camera_write >= camera_write_delay:

                # Write config
                self.writeCamS(self.cameraPath[1:3, 0:1])
                prev_camera_write = time.time()

                # Set new delay
                if self.cameraPath.shape[1] > 1:
                    # subtract next timestamp from current timestamp for delay
                    camera_write_delay = self.cameraPath[0][
                        1] - self.cameraPath[0][0]
                else:
                    camera_write_delay = 0

                # Pop current config
                self.cameraPath = self.cameraPath[0:3, 1:]
            # if self.gunReady and self.gunTurretReady:
            #     self.fireTheShot()
            # Display simulation.
            self.sim_out.update()
            # Hardware interface updates 50 times a second
            time.sleep(.02)

    def runR(self):
        """
        Check subscriber bus for path matrix. If path is found, clear cameraPath and replace with new path.
        If no path is found,
        """
        self.initSerial()

        prev_gun_write = time.time()
        gun_write_delay = 0

        prev_camera_write = time.time()
        camera_write_delay = 0

        t = threading.Thread(target=self.readStateR(), args=())
        t.start()

        while (True):
            # Update configuration states and publish them
            self.readStateR()
            self.publishState()

            # Pull path matrix from queue and replace existing matrices
            self.receivePath()

            # If path steps exist and sufficient time has elapsed since the last write, write again
            if  (self.gunPath is not None) and\
                    (self.gunPath.shape[1] != 0) and\
                    (time.time() - prev_gun_write >= gun_write_delay):

                # Write config
                self.writeGunR(self.gunPath[1:3, 0:1])
                prev_gun_write = time.time()

                # Set new delay
                if (self.gunPath.shape[1] > 1):
                    # subtract next timestamp from current timestamp for delay
                    gun_write_delay = self.gunPath[0][1] - self.gunPath[0][0]
                else:
                    gun_write_delay = 0

                # Pop current config
                self.gunPath = self.gunPath[0:3, 1:]

            if (self.cameraPath is not None) and\
                    (self.cameraPath.shape[1] != 0) and\
                    (time.time() - prev_camera_write >= camera_write_delay):

                # Write config
                self.writeCamR(self.cameraPath[1:3, 0:1])
                prev_camera_write = time.time()

                # Set new delay
                if (self.cameraPath.shape[1] > 1):
                    # subtract next timestamp from current timestamp for delay
                    camera_write_delay = self.cameraPath[0][
                        1] - self.cameraPath[0][0]
                else:
                    camera_write_delay = 0

                # Pop current config
                self.cameraPath = self.cameraPath[0:3, 1:]

            # Hardware interface updates 50 times a second
            time.sleep(.02)

    def run(self, is_sim):
        if is_sim:
            self.runS()
        else:
            self.runR()
Пример #3
0
class CameraMotion():
    def __init__(self):
        self.name = "camera"

        self.cameraTurret = CameraTurret()
        self.currentPos = np.array([0, 0]).reshape(2, 1)
        self.currentTarget = np.array([])
        self.gunPath = np.array([])

        self.Comms = Comms()
        self.Comms.add_subscriber_port('127.0.0.1', '3000', 'cState')
        self.Comms.add_publisher_port('127.0.0.1', '3002', 'cameraPath')
        self.Comms.add_subscriber_port('127.0.0.1', '3004', 'targetPos')

    def sendPath(self, path):
        self.Comms.define_and_send(self.name, 'cameraPath', path)

    def receive(self):
        try:
            self.currentTarget = self.Comms.get('targetPos').payload
        except queue.Empty:
            pass

        try:
            self.currentPos = self.Comms.get('cState').payload
            #print("new state: ", self.currentPos)
        except queue.Empty:
            pass

    def getFullSweep(self):
        duration = 5
        # Testing required to find q1_range and q2_range
        # Eventually should be declared as constants
        q_mat = self.cameraTurret.sweepPath(duration,
                                            q1_range=(-np.pi / 4, np.pi / 4),
                                            q2_range=(-np.pi / 4, np.pi / 4))

        # Make duplicate for reverse direction
        q_mat2 = self.cameraTurret.sweepPath(duration,
                                             q1_range=(-np.pi / 4, np.pi / 4),
                                             q2_range=(-np.pi / 4, np.pi / 4))
        q_mat2 = np.flip(q_mat2, 1)[:, 1:]
        q_size = int((q_mat2.size) / 3)
        # Change timestamps of reverse array
        for i in range(q_size):
            q_mat2[0, i] = duration + (duration - q_mat2[0, i])

        return np.concatenate((q_mat, q_mat2), axis=1), duration * 2

    def runS(self):
        sweepPath, duration = self.getFullSweep()
        sweepInit = np.array(sweepPath[1:3, 0]).reshape(2, 1)
        print(sweepInit)
        self.sendPath(sweepPath)

        startTime = time.time()
        #print(path, duration)

        # self.currentPos is incorrect, fixes should happen in hardware_interface
        while (True):
            self.receive()

            if self.currentTarget.size:
                targetPos = self.cameraTurret.inverseKin(self.currentTarget)
                targetPath = self.cameraTurret.scurvePath(
                    self.currentPos, targetPos, 10, 3, 0.1)
                #print(self.currentPos)
                self.sendPath(targetPath)

                # Hardcoded to duration of scurve * 2, may need to switch to when gun shoots (another connection)
                time.sleep(6)
                self.currentTarget = np.array([])
            else:
                if time.time() - startTime >= duration * 1.5:
                    # Destination is start of sweep path, hardcoded to (-np.pi/4, -np.pi/4)
                    # because sweep path init is hardcoded to this point
                    self.sendPath(
                        self.cameraTurret.scurvePath(self.currentPos,
                                                     sweepInit, 10, 2, 0.1))
                    # Hardcoded to duration of scurve * 2
                    time.sleep(4)
                    self.sendPath(sweepPath)
                    startTime = time.time()

                # Estimate of how long simulation actually takes vs expected duration
                # Need to test to get accurate scale
                # Also add conditional to see if target is found before duration is over

            time.sleep(0.02)

    def runSweep(self):
        sweepPath, duration = self.getFullSweep()
        startTime = 0

        while (True):
            if time.time() - startTime >= duration * 1.4:
                self.sendPath(sweepPath)
                startTime = time.time()

            time.sleep(0.02)

    def runR(self):
        pass

    def run(self, is_sim):
        if is_sim:
            # Change this function to run sweep only vs full motion mode
            self.runSweep()
        else:
            self.runR()
Пример #4
0
class GunMotion():
    def __init__(self):
        self.name = "gunmotion"

        # Current configuration of gun turret as reported by hardware
        self.actual_configuration = np.array([[0, 0]]).T
        self.dest_configuration = np.array([[0, 0]]).T

        # Configuration comparison tolerance (rad)
        self.tol = .025

        # First target indicator to skip delay
        self.first_targ = True

        # Time to wait for gun to fire
        self.delay = 2

        # queue of P0Ts to process
        self.targets = []

        self.gTurret = GunTurret(0, 0, 500, [-100, 50, 100])

        self.Comms = Comms()
        self.Comms.add_subscriber_port('127.0.0.1', '3001', 'gState')
        self.Comms.add_subscriber_port('127.0.0.1', '3004', 'targetPos')
        self.Comms.add_publisher_port('127.0.0.1', '3003', 'gunPath')

    def receive(self):
        """
        Reads gun turret configurations and the target position
        """
        try:
            self.actual_configuration = self.Comms.get('gState').payload
        except queue.Empty:
            pass
        try:
            self.targets.append(self.Comms.get('targetPos').payload)
        except queue.Empty:
            pass

    def publish(self, pathMatrix):
        """
        Publishes the gun path
        """
        self.Comms.define_and_send(self.name, 'gunPath', pathMatrix)

    def compareTol(self):
        # If there is a destination, see if configs match
        #for q, q_prime in zip(self.actual_configuration, self.dest_configuration):
        for i in range(self.actual_configuration.shape[0]):
            if (self.actual_configuration[i][0] >
                    self.dest_configuration[i][0] + self.tol
                    or self.actual_configuration[i][0] <
                    self.dest_configuration[i][0] - self.tol):
                return False

        # Delay for as long as the gun takes to wind up and shoot
        # For real demo, about 6 seconds
        if self.first_targ:
            self.first_targ = False
        else:
            time.sleep(self.delay)

        # Allow another path matrix to be sent
        return True

    def run(self, is_sim):
        """
        Checks the configuration of gun turret and the target, if a target is received, calculate the path matrix and
        delete the target to wait for the next one
        """
        while True:
            self.receive()
            """
            if self.prev_path_time is not None and time.time() > self.prev_path_time + self.delay_time:
                self.prev_path_time = None
                continue
            """
            if self.compareTol() and len(self.targets) > 0:
                self.gTurret.setP0T(self.targets.pop(0))
                q1, q2, toa, f = self.gTurret.inverseKin(print_time=True)
                pathMatrix = self.gTurret.scurvePath(
                    self.actual_configuration,
                    np.array([q1, q2]).reshape(2, 1), 10, 1.5, .05)

                self.dest_configuration = np.array([[
                    pathMatrix[1][pathMatrix.shape[1] - 1],
                    pathMatrix[2][pathMatrix.shape[1] - 1]
                ]]).T

                self.publish(pathMatrix)

            time.sleep(.02)