def __init__(self, camera_type):
super().__init__()
if camera_type != self.TYPE_T265 and camera_type != self.TYPE_D435:
raise Exception("Camera type invalid")
self.camera_type = camera_type
self.pipe = None
self.metadata = {}
self.threads = []
self.exit_threads = False
self.rb_i = RedisBridge(db=self.rd_cfg.databases['instruments'])
self.rb_r = RedisBridge(db=self.rd_cfg.databases['robot'])
self._init_frame_metadata_values()
self._setup_parameters()
self._setup_threads()
def run_main(self):
rb = RedisBridge(db=self.rd_cfg.databases['robot'])
self.conn = Connector(self.cfg.ap_to_redis,
self.cfg.connection_baudrate, 1, 0)
params = self.rd_cfg.robot
while not self.exit_threads:
try:
self.conn.send_heartbeat()
m = self.conn.get_callbacks(params)
if m is not None:
rb.add_key(m.to_json(), m.get_type(), to_json=False)
except:
pass
def __init__(self, **kwargs):
super().__init__(**kwargs)
self.rb = RedisBridge(db=self.rd_cfg.databases['instruments'])
self.keys = self.rd_cfg.generate_flat_keys('instruments')
self.conn = mavutil.mavlink_connection(
self.cfg.redis_to_ap,
autoreconnect = True,
source_system = 1,
source_component = 93,
baud=self.cfg.connection_baudrate,
force_connected=True
)
self.lock = threading.Lock()
default_msg_hz = 30.0
msg_hz = {
'send_vision_position_estimate': 30.0,
'send_obstacle_distance': 15.0
}
self.mavlink_thread = threading.Thread(target=self.mavlink_loop, args=[self.conn])
self.mavlink_thread.start()
self.sched = BackgroundScheduler()
logging.getLogger('apscheduler').setLevel(logging.ERROR)
self.data = {}
for k, v in self.keys.items():
try:
if v in msg_hz.keys():
seconds = 1.0/msg_hz[v]
else:
seconds = 1.0/default_msg_hz
func = getattr(self, v)
self.sched.add_job(self.send_message,
'interval',
seconds=seconds,
args=[func, k],
max_instances=1
)
except:
utils.progress(traceback)
else:
self.data[k] = None
def get_redis_data(database, paginate, paginate_it):
database = int(database)
paginate = int(paginate)
paginate_it = int(paginate_it)
rb = RedisBridge(db=int(database))
keys = rd_cfg.generate_flat_keys(database)
if isinstance(keys, dict):
keys = list(keys.keys())
start = (paginate_it - 1) * paginate
end = start + paginate
keys = keys[start:end]
data = {}
for key in keys:
data[key] = rb.get_key_by_string(key)
return json.dumps(data)
def _setup_parameters(self):
self.scale_factor = self.cfg.t265['scale_factor']
self.jump_threshold = self.cfg.t265['jump_threshold']
self.jump_speed_threshold = self.cfg.t265['jump_speed_threshold']
self.compass_enabled = self.cfg.t265['compass_enabled']
self.heading_north_yaw = None
self.rb_0 = RedisBridge(db=self.rd_cfg.databases['robot'])
if self.compass_enabled:
att = self.rb_0.get_key('ATTITUDE')
if att is not None:
self.heading_north_yaw = att['yaw']
else:
self.compass_enabled = False
self.logger.log_warn(
"Failed to enable compass, could not retrieve attitude yaw"
)
self._initialize_compute_vars()
# body offset - see initial script
self.metadata = ['enable_pose_stream']
class Camera(Base):
TYPE_T265 = 't265'
TYPE_D435 = 'd435'
ALIVE_FLAG = 'alive'
def __init__(self, camera_type):
super().__init__()
if camera_type != self.TYPE_T265 and camera_type != self.TYPE_D435:
raise Exception("Camera type invalid")
self.camera_type = camera_type
self.pipe = None
self.metadata = {}
self.threads = []
self.exit_threads = False
self.rb_i = RedisBridge(db=self.rd_cfg.databases['instruments'])
self.rb_r = RedisBridge(db=self.rd_cfg.databases['robot'])
self._init_frame_metadata_values()
self._setup_parameters()
self._setup_threads()
def start(self):
for t in self.threads:
t.start()
self._open_pipe()
while not self.exit_threads:
self._process_frames()
def stop(self):
self.exit_threads = True
try:
self.pipe.stop()
except:
self.logger.log_error("Pipe closing")
for t in self.threads:
try:
t.join()
except:
self.logger.log_warn("Thread failed to join")
def __enter__(self):
return self
def __exit__(self):
self.stop()
def _setup_parameters(self):
# Setup sensor-specific parameters
raise NotImplementedError
def _setup_threads(self):
self.threads = [Thread(target=self._send_metadata)]
def _send_metadata(self):
while not self.exit_threads:
try:
for key in self.metadata:
value = getattr(self, key)
self.rb_i.add_key(value, self.camera_type, key, expiry=5)
except Exception as e:
self.logger.log_error("Invalid redis metadata: %s" % str(e))
def _setup_save_dir(self):
try:
dir_path = dirname(dirname(dirname(realpath(__file__))))
datestamp = datetime.datetime.now().strftime("%Y_%m_%d")
self.save_data_dir = dir_path + "/" + self.cfg.save_data_dir + "/" + datestamp + "/"
if not os.path.exists(self.save_data_dir):
os.makedirs(self.save_data_dir)
except:
self.logger.log_error("Could not create directory: %s" % self.save_data_dir)
def _open_pipe(self):
raise NotImplementedError
def _process_frames(self):
# Process frames from pipe
raise NotImplementedError
def _init_frame_metadata_values(self):
self.frame_metadata_values = []
for k,v in rs.frame_metadata_value.__dict__.items():
if k.startswith('__') or k == 'name':
continue
self.frame_metadata_values.append(v)
def _get_gps_data(self):
gps_data_1 = self.rb_0.get_key('GPS_RAW_INT')
gps_data_2 = self.rb_0.get_key('GPS2_RAW')
gps = [None, None, None, None, None, None]
if gps_data_1 is not None:
# Use 1
gps[0] = gps_data_1['lat']
gps[1] = gps_data_1['lon']
gps[2] = gps_data_1['fix_type']
if gps_data_2 is not None:
# Use 2
gps[3] = gps_data_2['lat']
gps[4] = gps_data_2['lon']
gps[5] = gps_data_2['fix_type']
return gps
class CameraT265(Camera):
def __init__(self):
self.enable_pose_stream = False
super().__init__(Camera.TYPE_T265)
self._setup_save_dir()
def _setup_parameters(self):
self.scale_factor = self.cfg.t265['scale_factor']
self.jump_threshold = self.cfg.t265['jump_threshold']
self.jump_speed_threshold = self.cfg.t265['jump_speed_threshold']
self.compass_enabled = self.cfg.t265['compass_enabled']
self.heading_north_yaw = None
self.rb_0 = RedisBridge(db=self.rd_cfg.databases['robot'])
if self.compass_enabled:
att = self.rb_0.get_key('ATTITUDE')
if att is not None:
self.heading_north_yaw = att['yaw']
else:
self.compass_enabled = False
self.logger.log_warn(
"Failed to enable compass, could not retrieve attitude yaw"
)
self._initialize_compute_vars()
# body offset - see initial script
self.metadata = ['enable_pose_stream']
def _initialize_compute_vars(self):
self.prev_data = None
self.reset_counter = 1
self.current_confidence_level = None
# Initialize with camera orientation
self.H_aeroRef_T265Ref = np.array([[0, 0, -1, 0], [1, 0, 0, 0],
[0, -1, 0, 0], [0, 0, 0, 1]])
xr = m.radians(self.cfg.t265['camera_rot_x'])
yr = m.radians(self.cfg.t265['camera_rot_y'])
zr = m.radians(self.cfg.t265['camera_rot_z'])
self.H_T265body_aeroBody = (tf.euler_matrix(xr, yr, zr)).dot(
np.linalg.inv(self.H_aeroRef_T265Ref))
self.H_aeroRef_aeroBody = None
# V_aeroRef_aeroBody # vision speed estimate message
# H_aeroRef_PrevAeroBody # vision position delta message
self.frames = None
self.pose_estimate_data = None
def _setup_threads(self):
super()._setup_threads()
self.threads.append(Thread(target=self._save_pos_estimate))
def _save_pos_estimate(self):
csv_file = self.save_data_dir + 't265_pos_estimate.csv'
file_exists = os.path.exists(csv_file)
with open(csv_file, 'a+', newline='') as csvfile:
csvwriter = csv.writer(csvfile, delimiter=',')
if not file_exists:
header = [
'current_time_us', 'local_x_pos', 'local_y_pos',
'local_z_pos', 'roll_angle', 'pitch_angle', 'yaw_angle',
'covariance', 'reset_counter', 'gps_1_lat', 'gps_1_lon',
'gps_1_fix_type', 'gps_2_lat', 'gps_2_lon',
'gps_2_fix_type'
]
csvwriter.writerow(header)
while not self.exit_threads:
self.rb_i.add_key(self.pose_estimate_data,
self.camera_type,
'vision_position_estimate',
expiry=self.cfg.t265['save_redis_expiry'])
self._save_csv(csvwriter, self.pose_estimate_data)
def _save_csv(self, csvwriter, data):
if data:
try:
data += self._get_gps_data()
csvwriter.writerow(data)
except Exception as e:
self.logger.log_warn("Could not write pose data to csv: %s" %
e)
def _realsense_notification_callback(notif):
self.logger.log_info(notif)
def _open_pipe(self):
self.pipe = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.pose)
device = config.resolve(self.pipe).get_device()
pose_sensor = device.first_pose_sensor()
pose_sensor.set_notifications_callback(
self._realsense_notification_callback)
self.enable_pose_stream = True
self.pipe.start(config)
def _process_frames(self):
self.frames = self.pipe.wait_for_frames()
pose = self.frames.get_pose_frame()
if pose:
# Pose data consists of translation and rotation
data = pose.get_pose_data()
# Confidence level value from T265: 0-3, remapped to 0 - 100: 0% - Failed / 33.3% - Low / 66.6% - Medium / 100% - High
self.current_confidence_level = float(data.tracker_confidence *
100 / 3)
# In transformations, Quaternions w+ix+jy+kz are represented as [w, x, y, z]!
H_T265Ref_T265body = tf.quaternion_matrix([
data.rotation.w, data.rotation.x, data.rotation.y,
data.rotation.z
])
H_T265Ref_T265body[0][3] = data.translation.x * self.scale_factor
H_T265Ref_T265body[1][3] = data.translation.y * self.scale_factor
H_T265Ref_T265body[2][3] = data.translation.z * self.scale_factor
# Transform to aeronautic coordinates (body AND reference frame!)
self.H_aeroRef_aeroBody = self.H_aeroRef_T265Ref.dot(
H_T265Ref_T265body.dot(self.H_T265body_aeroBody))
# vision_speed_estimate_message
# Calculate GLOBAL XYZ speed (speed from T265 is already GLOBAL)
# V_aeroRef_aeroBody = tf.quaternion_matrix([1,0,0,0])
# V_aeroRef_aeroBody[0][3] = data.velocity.x
# V_aeroRef_aeroBody[1][3] = data.velocity.y
# V_aeroRef_aeroBody[2][3] = data.velocity.z
# V_aeroRef_aeroBody = H_aeroRef_T265Ref.dot(V_aeroRef_aeroBody)
# Check for pose jump and increment reset_counter
if self.prev_data is not None:
delta_translation = [
data.translation.x - self.prev_data.translation.x,
data.translation.y - self.prev_data.translation.y,
data.translation.z - self.prev_data.translation.z
]
delta_velocity = [
data.velocity.x - self.prev_data.velocity.x,
data.velocity.y - self.prev_data.velocity.y,
data.velocity.z - self.prev_data.velocity.z
]
position_displacement = np.linalg.norm(delta_translation)
speed_delta = np.linalg.norm(delta_velocity)
if (position_displacement > self.jump_threshold) or (
speed_delta > self.jump_speed_threshold):
if position_displacement > self.jump_threshold:
self.logger.log_warn("Position jumped by: %s" %
position_displacement)
elif speed_delta > self.jump_speed_threshold:
self.logger.log_warn("Speed jumped by: %s" %
speed_delta)
self._increment_reset_counter()
self.prev_data = data
# Take offsets from body's center of gravity (or IMU) to camera's origin into account
# if self.body_offset_enabled == 1:
# H_body_camera = tf.euler_matrix(0, 0, 0, 'sxyz')
# H_body_camera[0][3] = body_offset_x
# H_body_camera[1][3] = body_offset_y
# H_body_camera[2][3] = body_offset_z
# H_camera_body = np.linalg.inv(H_body_camera)
# H_aeroRef_aeroBody = H_body_camera.dot(H_aeroRef_aeroBody.dot(H_camera_body))
# Realign heading to face north using initial compass data
if self.compass_enabled:
self.H_aeroRef_aeroBody = self.H_aeroRef_aeroBody.dot(
tf.euler_matrix(0, 0, self.heading_north_yaw, 'sxyz'))
self._compute_pose_estimate(data)
def _increment_reset_counter(self):
if self.reset_counter >= 255:
self.reset_counter = 1
self.reset_counter += 1
def _compute_pose_estimate(self, data):
if self.H_aeroRef_aeroBody is not None:
current_time_us = int(round(time.time() * 1000000))
# Setup angle data
rpy_rad = np.array(
tf.euler_from_matrix(self.H_aeroRef_aeroBody, 'sxyz'))
# Setup covariance data, which is the upper right triangle of the covariance matrix, see here: https://files.gitter.im/ArduPilot/VisionProjects/1DpU/image.png
# Attemp #01: following this formula https://github.com/IntelRealSense/realsense-ros/blob/development/realsense2_camera/src/base_realsense_node.cpp#L1406-L1411
cov_pose = self.cfg.t265['linear_accel_cov'] * pow(
10, 3 - int(data.tracker_confidence))
cov_twist = self.cfg.t265['angular_vel_cov'] * pow(
10, 1 - int(data.tracker_confidence))
covariance = [
cov_pose, 0, 0, 0, 0, 0, cov_pose, 0, 0, 0, 0, cov_pose, 0, 0,
0, cov_twist, 0, 0, cov_twist, 0, cov_twist
]
self.pose_estimate_data = [
current_time_us,
self.H_aeroRef_aeroBody[0][3], # Local X position
self.H_aeroRef_aeroBody[1][3], # Local Y position
self.H_aeroRef_aeroBody[2][3], # Local Z position
rpy_rad[0], # Roll angle
rpy_rad[1], # Pitch angle
rpy_rad[2], # Yaw angle
covariance, # Row-major representation of pose 6x6 cross-covariance matrix
self.
reset_counter # Estimate reset counter. Increment every time pose estimate jumps.
]
self.logger.log_debug("Captured pose estimate data: %s" %
str(self.pose_estimate_data))
class __RedisToAPScripterExt(Scripter):
def __init__(self, **kwargs):
super().__init__(**kwargs)
self.rb = RedisBridge(db=self.rd_cfg.databases['instruments'])
self.keys = self.rd_cfg.generate_flat_keys('instruments')
self.conn = mavutil.mavlink_connection(
self.cfg.redis_to_ap,
autoreconnect = True,
source_system = 1,
source_component = 93,
baud=self.cfg.connection_baudrate,
force_connected=True
)
self.lock = threading.Lock()
default_msg_hz = 30.0
msg_hz = {
'send_vision_position_estimate': 30.0,
'send_obstacle_distance': 15.0
}
self.mavlink_thread = threading.Thread(target=self.mavlink_loop, args=[self.conn])
self.mavlink_thread.start()
self.sched = BackgroundScheduler()
logging.getLogger('apscheduler').setLevel(logging.ERROR)
self.data = {}
for k, v in self.keys.items():
try:
if v in msg_hz.keys():
seconds = 1.0/msg_hz[v]
else:
seconds = 1.0/default_msg_hz
func = getattr(self, v)
self.sched.add_job(self.send_message,
'interval',
seconds=seconds,
args=[func, k],
max_instances=1
)
except:
utils.progress(traceback)
else:
self.data[k] = None
# https://mavlink.io/en/messages/common.html#VISION_POSITION_ESTIMATE
def send_vision_position_estimate(self, current_time_us, x, y, z,
roll, pitch, yaw, covariance, reset_counter):
# self.connect(self.connection_string, self.connection_baudrate, self.source_system, self.source_component)
self.conn.mav.vision_position_estimate_send(
current_time_us, # us Timestamp (UNIX time or time since system boot)
x, # Local X position
y, # Local Y position
z, # Local Z position
roll, # Roll angle
pitch, # Pitch angle
yaw, # Yaw angle
covariance, # Row-major representation of pose 6x6 cross-covariance matrix
reset_counter # Estimate reset counter. Increment every time pose estimate jumps.
)
# def send_vision_position_delta_message(self, current_time_us, delta_time_us, delta_angle_rad, delta_position_m, current_confidence_level):
# conn.mav.vision_position_delta_send(
# current_time_us, # us: Timestamp (UNIX time or time since system boot)
# delta_time_us, # us: Time since last reported camera frame
# delta_angle_rad, # float[3] in radian: Defines a rotation vector in body frame that rotates the vehicle from the previous to the current orientation
# delta_position_m, # float[3] in m: Change in position from previous to current frame rotated into body frame (0=forward, 1=right, 2=down)
# current_confidence_level # Normalized confidence value from 0 to 100.
# )
# def send_vision_speed_estimate(self, current):
# self.conn.mav.vision_speed_estimate_send(
# current_time_us, # us Timestamp (UNIX time or time since system boot)
# V_aeroRef_aeroBody[0][3], # Global X speed
# V_aeroRef_aeroBody[1][3], # Global Y speed
# V_aeroRef_aeroBody[2][3], # Global Z speed
# covariance, # covariance
# reset_counter # Estimate reset counter. Increment every time pose estimate jumps.
# )
# https://mavlink.io/en/messages/common.html#OBSTACLE_DISTANCE
def send_obstacle_distance(self, current_time_us, sensor_type, distances, increment,
min_distance, max_distance, increment_f, angle_offset, mav_frame):
# self.connect(self.connection_string, self.connection_baudrate, self.source_system, self.source_component)
self.conn.mav.obstacle_distance_send(
current_time_us, # us Timestamp (UNIX time or time since system boot)
sensor_type, # sensor_type, defined here: https://mavlink.io/en/messages/common.html#MAV_DISTANCE_SENSOR
distances, # distances, uint16_t[72], cm
increment, # increment, uint8_t, deg
min_distance, # min_distance, uint16_t, cm
max_distance, # max_distance, uint16_t, cm
increment_f, # increment_f, float, deg
angle_offset, # angle_offset, float, deg
mav_frame # MAV_FRAME, vehicle-front aligned: https://mavlink.io/en/messages/common.html#MAV_FRAME_BODY_FRD
)
def run_main(self):
self.sched.start()
while not self.exit_threads:
with self.lock:
for k, _ in self.keys.items():
self.data[k] = self.rb.get_key_by_string(k)
# time.sleep(0.3)
# self.conn.send_heartbeat()
# m = self.conn.get_callbacks(['HEARTBEAT'])
# if m is None:
# continue
# self.logger.log_debug("Received callback: %s" % m)
# # utils.progress(m)
def mavlink_loop(self, conn, callbacks=['HEARTBEAT']):
while not self.exit_threads:
self.conn.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_ONBOARD_CONTROLLER,
mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
0,
0,
0)
m = self.conn.recv_match(type=callbacks, timeout=1, blocking=True)
if m is None:
continue
self.logger.log_debug("Received callback: %s" % m)
def send_message(self, func, key):
while not self.exit_threads:
with self.lock:
try:
value = self.data[key]
if value is not None:
func(*value)
except Exception as e:
self.logger.log_error("Could not send %s"%e)
def close_script(self):
try:
self.sched.shutdown()
self.mavlink_thread.join()
self.conn.close()
except:
pass
class __RedisToAPScripterExt(Scripter):
def __init__(self, connection, **kwargs):
super().__init__(**kwargs)
self.rb = RedisBridge(db=self.rd_cfg.databases['instruments'])
self.keys = self.rd_cfg.generate_flat_keys('instruments')
self.conn = connection
self.lock = threading.Lock()
default_msg_hz = 30.0
msg_hz = {
'send_vision_position_estimate': 30.0,
'send_obstacle_distance': 15.0
}
self.mavlink_thread = threading.Thread(target=self.mavlink_loop,
args=[self.conn])
self.mavlink_thread.start()
self.sched = BackgroundScheduler()
logging.getLogger('apscheduler').setLevel(logging.ERROR)
self.data = {}
for k, v in self.keys.items():
try:
if v in msg_hz.keys():
seconds = 1.0 / msg_hz[v]
else:
seconds = 1.0 / default_msg_hz
func = getattr(self.conn, v)
self.sched.add_job(self.send_message,
'interval',
seconds=seconds,
args=[func, k],
max_instances=1)
except:
utils.progress(traceback)
else:
self.data[k] = None
def run_main(self):
self.sched.start()
while not self.exit_threads:
with self.lock:
for k, _ in self.keys.items():
self.data[k] = self.rb.get_key_by_string(k)
time.sleep(0.3)
# self.conn.send_heartbeat()
# m = self.conn.get_callbacks(['HEARTBEAT'])
# if m is None:
# continue
# self.logger.log_debug("Received callback: %s" % m)
# # utils.progress(m)
def mavlink_loop(self, conn, callbacks=['HEARTBEAT']):
while not self.exit_threads:
self.conn.send_heartbeat()
m = self.conn.get_callbacks(callbacks)
if m is None:
continue
self.logger.log_debug("Received callback: %s" % m)
def send_message(self, func, key):
while not self.exit_threads:
with self.lock:
try:
value = self.data[key]
if value is not None:
func(*value)
except Exception as e:
self.logger.log_error("Could not send %s" % e)
def close_script(self):
try:
self.sched.shutdown()
self.mavlink_thread.join()
self.conn.disconnect()
except:
pass
def controls_tab():
rgb_status = get_switch_status(controls['rgb_capture'], False)
depth_status = get_switch_status(controls['depth_capture'], False)
infrared_status = get_switch_status(controls['infrared_capture'], False)
return render_template('main.html',
tab="controls",
tab_html="controls_tab.html",
controls=controls,
rgb_status=rgb_status,
depth_status=depth_status,
infrared_status=infrared_status)
rd_cfg = RedisConfig()
rb = RedisBridge(db=rd_cfg.databases['instruments'])
@app.route('/controls/set/<switch>/<value>')
def set_switch(switch, value):
msg = "Setting switch "
switch = int(switch)
value = int(value)
if switch == controls['ekf_source']:
msg += "EKF source to "
try:
switch = EKFSwitch()
switch.set_ekf_source(value)
except:
pass