示例#1
0
    def test_make_socket_connection_socket_error(self, mock_socket):
        mock_socket.side_effect = TcpSocketError
        self.assertEqual(Diagnostics.state, DiagState.CLOSED)

        # with self.assertRaises(DiagnosticsError):
        Diagnostics._make_socket_connection(5570)
        self.assertEqual(Diagnostics.state, DiagState.CLOSED)
 def activate(self):
     self.acquire_motors("Wheels")
     if not self.have_acquired("Wheels"):
         dg.print("Cannot access wheel motors for PID control.")
     self.begin_actuate()
     with self.condition:
         self.condition.notify()
示例#3
0
 def store_received(self, recvd_list):
     '''Store values received from remote, after checking that they are of the right format (4 angle values; three for the arm servos and 1 for the gripper). Once the values are checked, send them to the motors using the Actuator notification system. Return a reply in the form of a string to be sent back. If the values are invalid, return None.'''
     if len(recvd_list) != 4:
         return None
     try:
         thet_1 = int(recvd_list[0])
         thet_2 = int(recvd_list[1])
         thet_3 = int(recvd_list[2])
         gripval = int(recvd_list[3])
     except (ValueError, IndexError) as e:
         dg.print(str(e))
         return None
     else:
         # all values must be in the right range
         if thet_1 <= 180 and thet_1 >= -180\
            and thet_2 <= 180 and thet_2 >= -180\
            and thet_3 <= 180 and thet_3 >= -180\
            and gripval <= 180 and gripval >= -180:
             with self.condition:  # acquire lock
                 self.angle_1 = thet_1
                 self.angle_2 = thet_2
                 self.angle_3 = thet_3
                 self.angle_grp = gripval
                 self.condition.notify()  # tell the actuator thread
                 # that values are ready.
             return 'ACK'
         else:
             return 'ERR:RANGE'  # values in wrong range
示例#4
0
def register_unit(unitname):
    dg.print("Registering unit name {}...".format(unitname))
    cfg.overall_config.set_unitname(unitname)
    try:
        main_hostname = cfg.overall_config.main_hostname()
    except cfg.ConfigurationError as e:
        dg.print(str(e))
        return False
    main_ip = socket.gethostbyname(main_hostname)
    global client_conn, command_server  # needed to keep client_conn alive
    try:
        client_conn = rpyc.connect(main_ip, 18868)
    except ConnectionRefusedError as e:
        dg.print(str(e))
        return False
    # unit_ip = client_conn._config['endpoints'][0]
    unit_hostname, _ = client_conn._channel.stream.sock.getsockname()
    unit_ip = socket.gethostbyname(unit_hostname)
    cfg.overall_config.set_ip(unit_ip)
    dg.print("Found main unit '{}'".format(
        client_conn.root.get_service_name()))
    client_conn.root.register_unit_name(unitname)
    command_server = ThreadedServer(CommandService, port=18869)
    Thread(target=command_server.start, args=[]).start()
    client_conn.root.register_command_server(unitname, 18869)
    dg.print("Registered.")
    Thread(target=wait_for_close, args=[command_server]).start()
    return True
示例#5
0
    def release_camera(self):
        if self.have_camera():
            mgr.global_resources.release("Camera")
            self.camera = None
        else:
            dg.print("Warning (camera): release_camera called while not \
acquired.")
 def set_values(self, values):
     self.angle_grab = values[0]
     self.angle_top = values[1]
     self.angle_middle = values[2]
     self.angle_bottom = values[3]
     dg.print("Mock arm motors: values received: {}, {}, {}, {}".format(
         values[0], values[1], values[2], values[3]))
     self._set_angle()
示例#7
0
 def before_start_call(self):
     '''Call before the opmode's start() is executed.'''
     with self.opmode_lock:
         if not self.is_stopped():
             raise OpModeError(
                 'Cannot start {}: mode already active'.format(self.name))
         self.opmode_state = State.STARTING
     dg.print("Starting {} mode...".format(self.name))
示例#8
0
 def register_name(self, name):
     if name in type(self).goals_list:
         dg.print(
             "Warning: goal name '{}' already taken. Skipping...".format(
                 name))
         return
     type(self).goals_list[name] = self
     self.name = name
示例#9
0
 def register_name(self, name):
     '''Store opmode instance in opmode_list.'''
     if name in type(self).opmodes_list:
         dg.print(
             'WARNING: Operational mode name {} already registered. Skipping...'
             .format(name))
         return
     type(self).opmodes_list[name] = self
     self.name = name
示例#10
0
 def test_initialise_enabled(self, mock_cfg, mock_thread):
     mock_cfg.overall_config.diagnostics_enabled.return_value = True
     mock_cfg.overall_config.diagnostics_port.return_value = 5000
     thread = Mock()
     mock_thread.return_value = thread
     Diagnostics.initialise()
     mock_thread.assert_called_with(
         target=Diagnostics._make_socket_connection, args=[5000])
     thread.start.assert_called_with()
示例#11
0
 def activate(self):
     self.acquire_motors("Arm")
     if not self.have_acquired("Arm"):
         dg.print("Cannot access arm motors for inverse kinematics.")
     self.begin_actuate()
     with self.condition:
         self.condition.notify()  # provide initial angles to motors
     self.end_x, self.end_y, self.gamma = self._forward_kinematics(
         self.angle_bottom, self.angle_middle, self.angle_top)
示例#12
0
    def test_make_socket_wait_error(self, mock_socket):
        ms = Mock()
        mock_socket.return_value = ms
        ms.wait_for_connection.side_effect = TcpSocketError
        self.assertEqual(Diagnostics.state, DiagState.CLOSED)

        #with self.assertRaises(DiagnosticsError):
        Diagnostics._make_socket_connection(5570)
        self.assertEqual(Diagnostics.state, DiagState.CLOSED)
示例#13
0
    def set_values(self, values):
        x = values[0]
        y = values[1]
        dg.print("Mock wheel motors: values received: {}, {}".format(x, y))
        v_left = (y / self._ymax) + (1 / 2 * (x / self._xmax))
        v_right = (y / self._ymax) - (1 / 2 * (x / self._xmax))

        self._setMotorRight(v_right)
        self._setMotorLeft(v_left)
示例#14
0
    def begin_stream(self, source=None):
        '''Stream source (default is direct camera output) to the specified \
        port.'''
        if self.streaming:
            raise CameraUserError(
                'Already streaming: cannot start new stream.')
        if not self.have_camera():
            raise CameraUserError('Camera not acquired.')

        host = cfg.overall_config.get_connected_ip()
        if source is None:
            strm_port = cfg.cam_config.stream_port()
            strm_framerate = cfg.cam_config.stream_framerate()
            strm_width = cfg.cam_config.stream_frame_width()
            strm_height = cfg.cam_config.stream_frame_height()

            src_framerate = cfg.cam_config.capture_framerate()
            src_width = cfg.cam_config.capture_frame_width()
            src_height = cfg.cam_config.capture_frame_height()
        else:
            strm_port = source.stream_port()
            strm_framerate = source.stream_framerate()
            strm_width = source.stream_frame_width()
            strm_height = source.stream_frame_height()

            src_framerate = source.capture_framerate()
            src_width = source.capture_frame_width()
            src_height = source.capture_frame_height()

        device = cfg.cam_config.device()
        if device == 'rpicamsrc':
            compressor = 'omxh264enc'
            tune = ' '
        elif device == 'v4l2src' or device == 'avfvideosrc' \
            or device == "autovideosrc":
            compressor = 'x264enc'
            tune = ' tune=zerolatency '
        comm = 'appsrc ! videoconvert ! video/x-raw,width='+str(src_width)+\
            ',height='+str(src_height)+',framerate='+str(src_framerate)+\
            '/1,format=I420 ! '+compressor+tune+'! rtph264pay config-interval=1 pt=96 ! \
gdppay ! tcpserversink host='                             +host+' port='+str(strm_port)+' sync=false'

        self.stream_writer = cv2.VideoWriter(comm, cv2.CAP_GSTREAMER, 0,
                                             strm_framerate,
                                             (strm_width, strm_height), True)
        self.streaming = True

        if source is None:
            thread = Thread(target=self._stream, args=())
        else:
            thread = Thread(target=self._stream_nondefault, args=[source])
        thread.start()
        dg.print("Using camera device '{}' and encoder '{}'".format(
            device, compressor))
        dg.print("Streaming on IP address {} and port {}".format(
            host, strm_port))
示例#15
0
    def acquire_camera(self):
        '''Get shared access to the camera.'''
        if self.have_camera():
            raise CameraUserError('Already have camera access: cannot \
reacquire.')
        try:
            self.camera = mgr.global_resources.get_shared("Camera")
        except mgr.ResourceError as e:
            dg.print(str(e))
            raise CameraUserError('Could not get access to camera.')
示例#16
0
    def test_make_socket_connection(self, mock_socket, mock_thread):
        ms = Mock()
        mock_socket.return_value = ms
        self.assertEqual(Diagnostics.state, DiagState.CLOSED)

        Diagnostics._make_socket_connection(5570)
        mock_socket.assert_called_with(5570)
        ms.wait_for_connection.assert_called_with()
        ms.set_max_recv_bytes.assert_called_with(1024)
        self.assertEqual(Diagnostics.state, DiagState.ESTABLISHED)
        Diagnostics.state = DiagState.CLOSED
示例#17
0
 def test_print_closed(self):
     self.assertEqual(Diagnostics.state, DiagState.CLOSED)
     Diagnostics.socket = Mock()
     Diagnostics.buf = deque(maxlen=20)
     self.assertEqual(len(Diagnostics.buf), 0)
     Diagnostics.print("hi")
     self.assertEqual(len(Diagnostics.buf), 1)
     Diagnostics.print("hi again")
     self.assertEqual(len(Diagnostics.buf), 2)
     Diagnostics.socket.reply.assert_not_called()
     self.assertEqual(Diagnostics.state, DiagState.CLOSED)
示例#18
0
 def on_disconnect(self, conn):
     key_to_remove = None
     for key in self.__class__.unit_list.keys():
         _conn, _, _ = self.__class__.unit_list[key]
         if _conn == conn:
             dg.print("Unit {} disconnected.".format(key))
             key_to_remove = key
     if key_to_remove is not None:
         self.__class__.unit_list.pop(key_to_remove)
     else:
         dg.print("Warning: unknown unit disconnected.")
示例#19
0
 def test_print_established_error(self):
     self.assertEqual(Diagnostics.state, DiagState.CLOSED)
     Diagnostics.socket = Mock()
     Diagnostics.state = DiagState.ESTABLISHED
     Diagnostics.buf = deque(maxlen=20)
     Diagnostics.socket.reply.side_effect = TcpSocketError
     self.assertEqual(len(Diagnostics.buf), 0)
     #with self.assertRaises(DiagnosticsError):
     Diagnostics.print("hi")
     #self.assertEqual(len(Diagnostics.buf), 0)
     Diagnostics.socket.reply.assert_called_with("hi")
     self.assertEqual(Diagnostics.state, DiagState.CLOSED)
示例#20
0
 def before_stop_call(self):
     '''Call before the opmode's stop() is executed.'''
     with self.opmode_lock:
         if self.is_stopped():
             raise OpModeError('Cannot stop {}: already stopped'.format(
                 self.name))
         elif self.is_stopping():
             raise OpModeError(
                 'Cannot stop {}: busy processing previous stop request.'.
                 format(self.name))
         self.opmode_state = State.STOPPING
     dg.print("Stopping {} mode...".format(self.name))
示例#21
0
 def disconnect_internal(self):
     '''Close the socket and set the connection state to closed'''
     with self.state_lock:
         if self.state == ConnState.CLOSED:
             return
     dg.print("{}: disconnecting...".format(self.__class__.__name__))
     with self.state_lock:
         self.state = ConnState.CLOSED
     if self.socket is not None:
         self.socket.close()
         self.socket = None
     dg.print("{}: disconnected.".format(self.__class__.__name__))
示例#22
0
 def exposed_accept(self, command):
     dg.print("Command received from main unit: {}".format(command))
     try:
         parsed_args = parse_entry(command)
         Thread(target=launcher.call_action, args=[parsed_args]).start()
         with cli_wait:
             res = cli_wait.wait(10)  # wait for notification from mode.
             if not res:
                 return 'ERR'
         return 'ACK'
     except CommandError as e:
         dg.print(e)
示例#23
0
 def find_obj(self):
     frame = self.camera.get_frame()
     gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
     objects = []
     try:
         objects = self.cascade.detectMultiScale(gray, 1.1,
                                                 50)  #1.1,250 # 1.1,500
     except Exception as e:
         dg.print(str(e))
     for (x, y, w, h) in objects:
         cv2.rectangle(frame, (x, y), (x + w, y + h), (255, 0, 0), 2)
     self.update_frame(frame)  # for streaming
     return objects
示例#24
0
 def _setMotorLeft(self, power):
     int(power)
     if power < 0:
         pwm = int(self.duty_cycle * (1 + power))
         if pwm > self.duty_cycle:
             pwm = self.duty_cycle
     elif power > 0:
         pwm = int(self.duty_cycle * power)
         if pwm > self.duty_cycle:
             pwm = self.duty_cycle
     else:
         pwm = 0
     dg.print("Pwm value for left wheel motors: {}".format(pwm))
示例#25
0
    def release_motors(self, motor_set):
        '''Remove unique access to a set of motors so that they may be used
elsewhere.'''
        with self.list_lock:
            if motor_set in self.motor_list:
                self.motor_list.pop(motor_set)
                mgr.global_resources.release(motor_set)
                with self.condition:
                    self.release_was_called = True
                    self.condition.notify()
            else:
                dg.print(
                    "Warning (actuator): release_motors called on resource not acquired."
                )
示例#26
0
 def test_print_established(self):
     self.assertEqual(Diagnostics.state, DiagState.CLOSED)
     Diagnostics.socket = Mock()
     Diagnostics.state = DiagState.ESTABLISHED
     Diagnostics.buf = deque(maxlen=20)
     self.assertEqual(len(Diagnostics.buf), 0)
     Diagnostics.print("hi")
     self.assertEqual(len(Diagnostics.buf), 0)
     Diagnostics.socket.reply.assert_called_with("hi")
     Diagnostics.print("hi again")
     self.assertEqual(len(Diagnostics.buf), 0)
     Diagnostics.socket.reply.assert_called_with("hi again")
     self.assertEqual(Diagnostics.state, DiagState.ESTABLISHED)
     Diagnostics.state = DiagState.CLOSED
示例#27
0
 def store_received(self, recvd_list):
     if len(recvd_list) != 3:
         dg.print("Wrong length of recvd list: actual is {}".format(
             len(recvd_list)))
         return None
     try:
         thet_1 = int(recvd_list[0])
         thet_2 = int(recvd_list[1])
         thet_3 = int(recvd_list[2])
     except (ValueError, IndexError) as e:
         dg.print(str(e))
         return None
     else:
         try:
             self.clisock.write("{},{},{}".format(thet_1, thet_2, thet_3))
             self.clisock.read()  # perform the read to stay in sync
             time.sleep(0.02)  # strange error without this
             return 'ACK'
         except ClientSocketError as e:
             # self.run_on_connection_interrupted()
             dg.print("ERROR: " + str(e))
             dg.print("Bad values: {},{},{}".format(recvd_list[0],
                                                    recvd_list[1],
                                                    recvd_list[2]))
             return None
示例#28
0
    def _inverse_kinematics(self, x, y, gamma):
        '''
        x and y are measured relative to the base servo. They are the coordinates of the tip of the gripper.
        positive x is in the forward driving direction, positive y is away from the ground. 
        gamma is the orientation of the gripper. gamma=0 means the gripper segment is parallel to the ground.
        Note that self.gamma is the current orientation, while gamma is the orientation to set.
        gamma=-pi/2 means the gripper is pointing at the ground.
        angles theta1,theta2,theta3 are the servo angles at base,middle,top along the arm.
        they are assumed to be -pi/2 < theta < pi/2.
        angles are converted to and returned in degrees.
        '''
        arm_l1 = self.arm_l1
        arm_l2 = self.arm_l2
        arm_l3 = self.arm_l3

        x2R = x - arm_l3 * math.cos(
            gamma)  # x for the tip of the second segment
        y2R = y - arm_l3 * math.sin(
            gamma)  # y for the tip of the second segment
        ct2 = (((x2R**2) + (y2R**2)) -
               ((arm_l1**2) +
                (arm_l2**2))) / (2 * arm_l1 * arm_l2)  # cos(theta2)
        #print("GAMMA: {}".format(math.degrees(gamma)))
        #print("COSTHETA2: {}".format(ct2))
        try:
            if gamma > 0:  # gripper looking up
                # print(ct2)
                thet2 = math.acos(
                    ct2)  # elbow down configuration for 2R manipulator problem
            else:
                # print(ct2)
                thet2 = -math.acos(ct2)  # elbow up configuration
            st2 = math.sin(thet2)
        except Exception as e:
            dg.print("Sample unreachable")
            return None

        A = arm_l1 + arm_l2 * ct2
        B = arm_l2 * st2
        AB2 = A**2 + B**2
        ct1 = (x2R * A + y2R * B) / AB2
        st1 = (y2R * A - x2R * B) / AB2
        if ct1 > 0:  # principle angles
            thet1 = math.asin(st1)
        else:
            thet1 = math.pi - math.asin(st1)

        thet3 = gamma - thet2 - thet1
        return (math.degrees(thet1), math.degrees(thet2), math.degrees(thet3))
示例#29
0
    def __init__(self):
        Resource.__init__(self)
        self.policy = Policy.UNIQUE
        self.register_name("Arm")
        dg.print("Mock arm motors settings:")

        servo1_pin = cfg.motor_config.get_pin("Arm", "Servo1")
        servo2_pin = cfg.motor_config.get_pin("Arm", "Servo2")
        servo3_pin = cfg.motor_config.get_pin("Arm", "Servo3")
        gripper_pin = cfg.motor_config.get_pin("Arm", "Gripper")

        dg.print("    Servo 1 pin: {}".format(servo1_pin))
        dg.print("    Servo 2 pin: {}".format(servo2_pin))
        dg.print("    Servo 3 pin: {}".format(servo3_pin))
        dg.print("    Gripper pin: {}".format(gripper_pin))
示例#30
0
 def stop(self, args):
     if self.connection_active():
         try:
             self.disconnect()
         except ReceiverError as e:
             raise OpModeError(str(e))
     redirect.stop()
     try:
         if self.clisock.is_open(
         ):  # only way to check if unit is reachable
             unit.send_command(self.attached_unit, "STOP DepCamera")
             self.clisock.close()
     except unit.UnitError as e:
         dg.print(
             "Warning: sending command 'STOP DepCamera' to unit {} failed.".
             format(self.attached_unit))