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
