def check_readings(self): """Update the instrument readings to the latest""" if self._mav.port.closed: print("Reading from a closed serial port") return isnew = self._read_buffer() # update sensor readings if a reading has changed if isnew: self.timestamp = timestamp() if self._messages['AHRS2'] is not None: # convert from radians to degrees yaw_deg = self._messages['AHRS2'].yaw * 180 / pi # convert from (-180, 180) range to (0, 360) range if yaw_deg < 0: yaw_deg += 360 self.heading = yaw_deg rc = self._messages['SERVO_OUTPUT_RAW'] if rc is not None: self.rc_out = [rc.servo1_raw, rc.servo2_raw, rc.servo3_raw, rc.servo4_raw, rc.servo5_raw, rc.servo6_raw, rc.servo7_raw, rc.servo8_raw] self.rc_out = np.array(self.rc_out) sp_msg = self._messages['SCALED_PRESSURE2'] if sp_msg is not None: # converte from hPa to dBar self.depth = sp_msg.press_diff * .01
def __init__(self, port): """Serial connection specifications port is a string specifying serial port location of the pixhawk Linux: '/dev/serial/by-id/usb-3D_Robotics_PX4_FMU_v2.x_0-if00' Mac OSX: '/dev/tty.usbmodem1' Windows: 'COM7' *Except for Linux, numbers at the end of these strings may change* """ self.baud = 115200 self.port = port # currently only know how to request a lot of data self.data_stream_ID = mavutil.mavlink.MAV_DATA_STREAM_ALL #MAV_DATA_STREAM_ALL self.data_rate = 10 # action rate, Hz # Define messages of interest from the pixhawk self.message_types = ['AHRS2', 'SERVO_OUTPUT_RAW', 'SCALED_PRESSURE2'] # Define where to save readings self.timestamp = timestamp() self.heading = empty_value self.depth = empty_value self.rc_command = empty_value * np.ones(4, dtype=np.int_) self.rc_out = empty_value * np.ones(8, dtype=np.int_) self.mode = '' # internal object to maintain io with pixhawk self._mav = None # internal dictionary for saving raw messages self._messages = dict.fromkeys(self.message_types)
def check_readings(self): """Take a picture if avalible""" if self.input_pipe is not None: self.serialize() if not self.cam.is_opened(): print('Camera is not open') return self.zedStatus = self.cam.grab(self.runtime_param) #run camera if self.zedStatus == sl.ERROR_CODE.SUCCESS: isnew = True self.image_time = timestamp() self.cam.retrieve_image(self._image, sl.VIEW.VIEW_LEFT) self.cam.retrieve_measure(self._depth, sl.MEASURE.MEASURE_DEPTH) self.image = self._image.get_data() self.depth = self._depth.get_data() # remove nans from depth map self.depth[np.isnan(self.depth)] = self.max_depth # limit maximal value of depth map self.depth[self.depth > self.max_depth] = self.max_depth # convert from float to int8 self.depth = self.depth * 255 / self.max_depth self.depth = self.depth.astype(np.uint8) #self.serialize() else: isnew = False return isnew
def __init__(self, input_pipe=None, output_pipes=None): """Basic initilization of camera""" self.input_pipe = input_pipe # make output pipe a list by default if not isinstance(output_pipes, (list, )): output_pipes = [output_pipes] self.output_pipes = output_pipes # These are the variables that will be shared across all zed nodes self.image_time = None self.image = None self.depth = None # These variables are used only in the zed node opening the camera # create a save directory, drop ms from datestring self.savedir = '_'.join(timestamp().split('_')[:-1]) self.savedir = os.path.join(os.getcwd(), self.savedir) self.init = sl.InitParameters(**param) self.cam = sl.Camera() self.zed_param = None self.zedStatus = None self.runtime_param = None self._image = sl.Mat() self._depth = sl.Mat() self.max_depth = 10 # max depth in map, meters
def __init__(self, writeonly=False): """Basic initilization of camera""" self.writeonly = writeonly # create a save directory, drop ms from datestring self.savedir = '_'.join(timestamp().split('_')[:-1]) self.savedir = os.path.join(os.getcwd(), self.savedir) self.init = sl.InitParameters(**param) self.cam = sl.Camera() self.zed_param = None self.zedStatus = None self.runtime_param = None self._image = sl.Mat() self._depth = sl.Mat() self.image = None self.depth = None self.image_time = None
def check_readings(self): """Take a picture if avalible""" if not self.cam.is_opened(): print('Camera is not open') return self.zedStatus = self.cam.grab(self.runtime_param) #run camera if self.zedStatus == sl.ERROR_CODE.SUCCESS: isnew = True self.image_time = timestamp() self.cam.retrieve_image(self._image, sl.VIEW.VIEW_LEFT) self.cam.retrieve_measure(self._depth, sl.MEASURE.MEASURE_DEPTH) self.image = self._image.get_data() self.depth = self._depth.get_data() else: isnew = False return isnew
def spin(self): """Process one frame at a time, put into an infinite loop""" if self.sim_gen is None: # read data from stream object and process it data = self.stream.read(self.buffer_size) recorded_data = self._buf_to_np(data) else: recorded_data = next(self.sim_gen) processed_data = self.process(recorded_data) # send result out over lcm msg = audio_data_t() msg.timestamp = timestamp() msg.num_channels, msg.num_samples = processed_data.shape msg.fc = self.fc msg.re_samples = processed_data.real msg.im_samples = processed_data.imag self.lc.publish("ACOUSITCS", msg.encode())
def check_readings(self): """Take a picture if avalible""" if self.input_dir is not None: #get image from video ret, image = self.image_reader.read() if ret: self.image = image #get depth from video ret, depth = self.depth_reader.read() if ret: self.depth = depth return ret # Once we get to this part of the code we are working with zed camera if not self.cam.is_opened(): print('Camera is not open') return self.zedStatus = self.cam.grab(self.runtime_param) #run camera if self.zedStatus == sl.ERROR_CODE.SUCCESS: isnew = True self.image_time = timestamp() self.cam.retrieve_image(self._image, sl.VIEW.VIEW_LEFT) self.cam.retrieve_measure(self._depth, sl.MEASURE.MEASURE_DEPTH) # save image image = self._image.get_data() # first convert from RGBA to RGB self.image = cv2.cvtColor(image, cv2.COLOR_RGBA2RGB) # save depth map self.depth = self._depth.get_data() # remove nans from depth map self.depth[np.isnan(self.depth)] = self.max_depth # limit maximal value of depth map self.depth[self.depth > self.max_depth] = self.max_depth # convert from float to int8 self.depth = self.depth * 255 / self.max_depth self.depth = self.depth.astype(np.uint8) else: isnew = False return isnew
def __init__(self, input_dir=None): """Basic initilization of camera""" # zed node can be setup to read from saved videos self.input_dir = input_dir self.image_reader = None self.depth_reader = None # These are the variables that will be used by mission self.image_time = None self.image = None self.depth = None # These variables are used only in the zed node opening the camera # create a save directory, drop ms from datestring self.savedir = '_'.join(timestamp().split('_')[:-1]) self.savedir = os.path.join(os.getcwd(), self.savedir) # set up camera only if we need to work with live feed if input_dir is None: self.init = sl.InitParameters(**param) self.cam = sl.Camera() self._image = sl.Mat() self._depth = sl.Mat() else: self.init = None self.cam = None self._image = None self._depth = None self.zed_param = None self.zedStatus = None self.runtime_param = None self.max_depth = 10 # max depth in map, meters self.codec = cv2.VideoWriter_fourcc(*'DIVX') self.depth_writer = None self.image_writer = None