def loadCameraInfo(self):
CameraInfoManager.loadCameraInfo(self)
# load all calibration files for all zoom levels
self._camera_infos = dict()
for zoom_level in self._zoom_levels:
resolved_url = resolveURL(self._calibration_url_template % zoom_level, self.cname)
url_type = parseURL(resolved_url)
if url_type == URL_empty:
raise CameraInfoError('Zoom camera cannot use default calibration URLs.')
rospy.loginfo('camera calibration URL for zoom level %d: %s' % (zoom_level, resolved_url))
if url_type == URL_file:
self._camera_infos[zoom_level] = loadCalibrationFile(resolved_url[7:], self.cname)
elif url_type == URL_package:
filename = getPackageFileName(resolved_url)
if filename == '': # package not resolved
raise CameraInfoMissingError('Calibration package missing.')
self._camera_infos[zoom_level] = loadCalibrationFile(filename, self.cname)
else:
rospy.logerr("Invalid camera calibration URL: " + resolved_url)
self._camera_infos[zoom_level] = CameraInfo()
if len(self._camera_infos.keys()) < 2:
raise CameraInfoError('Interpolating zoom camera info manager needs at least two calibrations to exist.')
def from_file(cls):
import rospy
import camera_info_manager as cim
camera_info_url = rospy.get_param('camera/info_url')
camera_name = rospy.get_param('camera/name')
rospy.loginfo('Initializing camera {0} from {1}.'.format(
camera_name,
camera_info_url))
if camera_info_url.upper().startswith('FILE://'):
camera_info = cim.loadCalibrationFile(camera_info_url[7:],
camera_name)
else:
raise NotImplementedError("Only urls of file:// type are supported for now")
# if we want another url type, rip from here:
# /opt/ros/indigo/lib/python2.7/dist-packages/camera_info_manager/camera_info_manager.py:
# CameraInfoManager._loadCalibration
# make sure it was successful
if camera_info.K[0] == 0.0:
raise RuntimeError("Could not load camera info. Bad url? (url={0})".format(camera_info_url))
return cls(camera_name, camera_info)
def loadCameraInfo(self):
CameraInfoManager.loadCameraInfo(self)
# load all calibration files for all zoom levels
self._camera_infos = dict()
for zoom_level in self._zoom_levels:
resolved_url = resolveURL(
self._calibration_url_template % zoom_level, self.cname)
url_type = parseURL(resolved_url)
if url_type == URL_empty:
raise CameraInfoError(
'Zoom camera cannot use default calibration URLs.')
rospy.loginfo('camera calibration URL for zoom level %d: %s' %
(zoom_level, resolved_url))
if url_type == URL_file:
self._camera_infos[zoom_level] = loadCalibrationFile(
resolved_url[7:], self.cname)
elif url_type == URL_package:
filename = getPackageFileName(resolved_url)
if filename == '': # package not resolved
raise CameraInfoMissingError(
'Calibration package missing.')
self._camera_infos[zoom_level] = loadCalibrationFile(
filename, self.cname)
else:
rospy.logerr("Invalid camera calibration URL: " + resolved_url)
self._camera_infos[zoom_level] = CameraInfo()
if len(self._camera_infos.keys()) < 2:
raise CameraInfoError(
'Interpolating zoom camera info manager needs at least two calibrations to exist.'
)
def init_from_yaml(self, init_filename):
try:
ci = loadCalibrationFile(filename=init_filename,
cname=self.camera_name)
except IOError:
print 'WARNING: {:s} camera info could not be loaded from {:s}'.\
format(self.camera_name, init_filename)
self.im_size = (ci.width, ci.height)
self.dist_model = ci.distortion_model
self.header = ci.header
self.K = np.asarray(ci.K).reshape((3, 3))
self.D = np.asarray(ci.D)
self.D = self.D[np.newaxis, :]
self.R = np.asarray(ci.R).reshape((3, 3))
self.P = np.asarray(ci.P).reshape((3, 4))
self.P = self.P[:3, :3]
def __init__(self):
super().__init__('tello_driver')
self.declare_parameter("camera_calibration",
"tello_driver/config/960x720.yaml")
self.declare_parameter("camera_frame", "camera_front")
calib_path = self.get_parameter(
"camera_calibration").get_parameter_value().string_value
camera_frame = self.get_parameter(
"camera_front").get_parameter_value().string_value
self.drone = tellopy.Tello()
self.drone.subscribe(self.drone.EVENT_LOG_DATA,
self.flight_data_handler)
self.drone.connect()
self.drone.wait_for_connection(60.0)
# self.container = av.open(self.drone.get_video_stream())
self._cmd_vel_sub = self.create_subscription(Twist, "cmd_vel",
self.cmd_vel_callback,
qos_profile_sensor_data)
self.bridge = CvBridge()
self._image_pub = self.create_publisher(Image, "image_raw", 10)
self._imu_pub = self.create_publisher(Imu, "imu", 10)
self._odom_pub = self.create_publisher(Odometry, "odom", 1)
self._state_pub = self.create_publisher(TelloState, "tello/state", 1)
self._action_srv = self.create_service(TelloAction, 'tello/action',
self.action_callback)
self.caminfo = cim.loadCalibrationFile(calib_path, camera_frame)
self.caminfo.header.frame_id = camera_frame
self._caminfo_pub = rospy.Publisher('camera_info',
CameraInfo,
queue_size=1,
latch=True)
self._caminfo_pub.publish(self.caminfo)
self.frame_thread = threading.Thread(target=self.framegrabber_loop)
self.frame_thread.start()
timer_period = 0.01 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
def __init__(self, data_dir):
self.data_dir = osp.join(osp.expanduser(data_dir))
# RGB camera info
cinfo_filename = cinfo_manager.getPackageFileName(
'package://contactdb_utils/calibrations/kinect.yaml')
kinect_cinfo = cinfo_manager.loadCalibrationFile(
cinfo_filename, 'kinect')
self.rgb_im_size = (kinect_cinfo.width, kinect_cinfo.height)
self.rgb_K = np.reshape(kinect_cinfo.K, (3, 3))
# 3D transform between RGB and thermal
stereo_filename = osp.join('..', 'calibrations', 'stereo.pkl')
with open(stereo_filename, 'rb') as f:
d = pickle.load(f)
T = np.eye(4)
T[:3, :3] = d['R']
T[:3, 3:] = d['T']
self.T_rgb_t = T
def map_texture(object_name, session_name, base_dir, models_dir,
debug_mode=False, show_textured_mesh=False,
depth_thresh_for_visibility=1e-2, depth_thresh_for_discontinuity=0.035,
max_vertex_normal_angle=70, real_depth_maps=False):
data_dir = osp.join(base_dir, session_name, object_name)
try:
with open(osp.join(data_dir, 'object_name.txt'), 'r') as f:
object_name = f.readline().rstrip()
except IOError:
print('{:s} does not have object_name.txt, skipping'.format(data_dir))
return
# read mesh file
mesh_filename = osp.join(models_dir, '{:s}.ply'.format(object_name))
mesh = open3d.read_triangle_mesh(mesh_filename)
if not mesh.has_vertex_normals():
mesh.compute_vertex_normals()
# names of views
rgb_im_dir = osp.join(data_dir, 'thermal_images')
pose_dir = osp.join(data_dir, 'poses')
names = []
for filename in os.listdir(pose_dir):
if '.txt' not in filename:
continue
if 'camera_pose' not in filename:
continue
name = '_'.join(filename.split('.')[0].split('_')[2:])
names.append(name)
names = sorted(names)
# camera extrinsics
Ts = []
for name in names:
filename = osp.join(pose_dir, 'camera_pose_{:s}.txt'.format(name))
T = np.eye(4)
with open(filename, 'r') as f:
f.readline()
T[0, 3] = float(f.readline().strip())
T[1, 3] = float(f.readline().strip())
T[2, 3] = float(f.readline().strip())
f.readline()
f.readline()
T[0, :3] = [float(v) for v in f.readline().strip().split()]
T[1, :3] = [float(v) for v in f.readline().strip().split()]
T[2, :3] = [float(v) for v in f.readline().strip().split()]
Ts.append(np.linalg.inv(T))
# RGB images
rgb_ims = []
im_intensities = []
for name in names:
rgb_im = open3d.read_image(osp.join(rgb_im_dir, '{:s}.png'.format(name)))
rgb_im = np.asarray(rgb_im)
rgb_ims.append(rgb_im)
im_shape = rgb_im.shape
intensity = rgb_im[:200, :200, 0].mean()
im_intensities.append(intensity)
target_intensity = np.mean(im_intensities)
for i, rgb_im in enumerate(rgb_ims):
rgb_im = rgb_im * target_intensity / im_intensities[i]
rgb_im = np.clip(rgb_im, a_min=0, a_max=255)
rgb_ims[i] = open3d.Image(rgb_im.astype(np.uint8))
# camera intrinsic
cinfo_filename = cinfo_manager.getPackageFileName(
'package://contactdb_utils/calibrations/boson.yaml')
cinfo = cinfo_manager.loadCalibrationFile(cinfo_filename, 'boson')
h_scaling = float(im_shape[0]) / cinfo.height
w_scaling = float(im_shape[1]) / cinfo.width
K = np.asarray(cinfo.K)
K[0] *= w_scaling
K[2] *= w_scaling
K[4] *= h_scaling
K[5] *= h_scaling
K = K.reshape((3,3))
intrinsic = open3d.PinholeCameraIntrinsic(im_shape[1], im_shape[0], K[0,0],
K[1,1], K[0,2], K[1,2])
if real_depth_maps: # use registered Kinect depthmaps
depth_im_dir = osp.join(data_dir, 'depth_images')
depth_ims = []
for name in names:
depth_im_filename = osp.join(depth_im_dir, '{:s}.png'.format(name))
depth_im = open3d.read_image(depth_im_filename)
depth_im = np.uint16(fill_holes(depth_im))
depth_ims.append(depth_im)
else: # create depth maps by rendering
depth_ims = render_depth_maps(mesh_filename, intrinsic, Ts)
# create RGB-D images
rgbds = []
for depth_im, rgb_im, T in zip(depth_ims, rgb_ims, Ts):
depth_im = open3d.Image(depth_im)
rgbds.append(open3d.create_rgbd_image_from_color_and_depth(rgb_im,
depth_im, convert_rgb_to_intensity=False))
if debug_mode:
pc = open3d.create_point_cloud_from_rgbd_image(rgbds[-1], intrinsic)
tmesh = copy(mesh)
tmesh.transform(T)
geoms = [pc]
if show_textured_mesh:
geoms.append(tmesh)
open3d.draw_geometries(geoms)
# create trajectory for texture mapping
traj = open3d.PinholeCameraTrajectory()
traj.extrinsic = open3d.Matrix4dVector(np.asarray(Ts))
traj.intrinsic = intrinsic
# do texture mapping!
option = open3d.ColorMapOptmizationOption()
option.maximum_iteration = 300
option.depth_threshold_for_visiblity_check = depth_thresh_for_visibility
option.depth_threshold_for_discontinuity_check = \
depth_thresh_for_discontinuity
option.half_dilation_kernel_size_for_discontinuity_map = 0
option.max_angle_vertex_normal_camera_ray = max_vertex_normal_angle
open3d.color_map_optimization(mesh, rgbds, traj, option)
if not debug_mode: # write result as a PLY file
mesh_filename = osp.join(data_dir, 'thermal_images',
'{:s}_textured.ply'.format(object_name))
open3d.write_triangle_mesh(mesh_filename, mesh)
print('Written {:s}'.format(mesh_filename))
if show_textured_mesh:
show_object_mesh(data_dir)
def __init__(self):
# Fetch parameters
self.local_cmd_client_port = int(
rospy.get_param('~local_cmd_client_port', 8890))
self.local_vid_server_port = int(
rospy.get_param('~local_vid_server_port', 6038))
self.tello_ip = rospy.get_param('~tello_ip', '192.168.10.1')
self.tello_cmd_server_port = int(
rospy.get_param('~tello_cmd_server_port', 8889))
self.connect_timeout_sec = float(
rospy.get_param('~connect_timeout_sec', 10.0))
self.stream_h264_video = bool(
rospy.get_param('~stream_h264_video', False))
self.tello_sdk = bool(rospy.get_param('~tello_sdk', True))
self.bridge = CvBridge()
self.frame_thread = None
self.cam_fps = rospy.get_param('~cam_fps', 40)
default_calib_path = RosPack().get_path(
'tello_driver') + '/cam_calib/default.yaml'
self.calib_path = rospy.get_param('~camera_calib', default_calib_path)
self.caminfo = cim.loadCalibrationFile(self.calib_path, 'camera_front')
self.caminfo.header.frame_id = rospy.get_param('~camera_frame',
'camera_front')
# Connect to drone
log = RospyLogger('Tello')
log.set_level(self.LOG_INFO)
super(TelloNode,
self).__init__(local_cmd_client_port=self.local_cmd_client_port,
local_vid_server_port=self.local_vid_server_port,
tello_ip=self.tello_ip,
tello_cmd_server_port=self.tello_cmd_server_port,
tello_sdk=self.tello_sdk,
log=log)
rospy.loginfo('Connecting to drone @ %s:%d' % self.tello_addr)
self.connect()
try:
self.wait_for_connection(timeout=self.connect_timeout_sec)
except error.TelloError as err:
rospy.logerr(str(err))
rospy.signal_shutdown(str(err))
self.quit()
return
rospy.loginfo('Connected to drone')
rospy.on_shutdown(self.cb_shutdown)
# Setup dynamic reconfigure
self.cfg = None
self.srv_dyncfg = Server(TelloConfig, self.cb_dyncfg)
# Setup topics and services
# NOTE: ROS interface deliberately made to resemble bebop_autonomy
self.pub_status = rospy.Publisher('status',
TelloStatus,
queue_size=1,
latch=True)
self.pub_odom = rospy.Publisher('odom',
Odometry,
queue_size=1,
latch=True)
self.pub_caminfo = rospy.Publisher('camera/camera_info',
CameraInfo,
queue_size=1,
latch=True)
if self.stream_h264_video:
self.pub_image_h264 = rospy.Publisher('camera/image_raw/h264',
H264Packet,
queue_size=10)
else:
self.pub_image_raw = rospy.Publisher('camera/image_raw',
Image,
queue_size=10)
self.sub_ap_connect = rospy.Subscriber('ap_connect', Empty,
self.cb_ap_connect)
self.sub_videomode = rospy.Subscriber('toggle_cam', Empty,
self.cb_videomode)
self.sub_takeoff = rospy.Subscriber('takeoff', Empty, self.cb_takeoff)
self.sub_throw_takeoff = rospy.Subscriber('auto_takeoff', Empty,
self.cb_throw_takeoff)
self.sub_land = rospy.Subscriber('land', Empty, self.cb_land)
# TODO self.sub_palm_land = rospy.Subscriber('palm_land', Empty, self.cb_palm_land)
self.sub_emergency = rospy.Subscriber('emergency',
Empty,
self.cb_emergency,
queue_size=1)
self.sub_flattrim = rospy.Subscriber('flattrim', Empty,
self.cb_flattrim)
self.sub_flip = rospy.Subscriber('flip', UInt8, self.cb_flip)
self.sub_cmd_vel = rospy.Subscriber('cmd_vel',
Twist,
self.cb_cmd_vel,
queue_size=1)
self.sub_fast_mode = rospy.Subscriber('pilot_mode', Empty,
self.cb_fast_mode)
self.subscribe(self.EVENT_FLIGHT_DATA, self.cb_status_telem)
self.subscribe(self.EVENT_LOG_DATA, self.cb_status_log)
if self.stream_h264_video:
self.start_video()
self.subscribe(self.EVENT_VIDEO_FRAME, self.cb_h264_frame)
else:
if self.tello_sdk:
self.stream_on()
self.subscribe(self.EVENT_VIDEO_FRAME, self.cb_frame)
else:
self.frame_thread = threading.Thread(
target=self.framegrabber_loop)
self.frame_thread.start()
# NOTE: odometry from parsing logs might be possible eventually,
# but it is unclear from tests what's being sent by Tello
# - https://github.com/Kragrathea/TelloLib/blob/master/TelloLib/Tello.cs#L1047
# - https://github.com/Kragrathea/TelloLib/blob/master/TelloLib/parsedRecSpecs.json
# self.pub_odom = rospy.Publisher(
# 'odom', UInt8MultiArray, queue_size=1, latch=True)
# self.pub_odom = rospy.Publisher(
# 'odom', Odometry, queue_size=1, latch=True)
#self.subscribe(self.EVENT_LOG, self.cb_odom_log)
rospy.loginfo('Tello driver node ready')
def generate_object_masks(object_name,
session_name,
base_dir,
models_dir=default_models_dir,
dilate_size=default_dilation):
logger = logging.getLogger(__name__)
data_dir = osp.join(base_dir, session_name, object_name)
try:
with open(osp.join(data_dir, 'object_name.txt'), 'r') as f:
object_name = f.readline().rstrip()
except IOError:
logger.warn(
'{:s} does not have object_name.txt, skipping'.format(data_dir))
return
mesh_filename = osp.join(models_dir, '{:s}.ply'.format(object_name))
# names of views
pose_dir = osp.join(data_dir, 'poses')
names = []
for filename in os.listdir(pose_dir):
if '.txt' not in filename:
continue
if 'camera_pose' not in filename:
continue
name = '_'.join(filename.split('.')[0].split('_')[2:])
names.append(name)
names = sorted(names)
# camera extrinsics
Ts = []
for name in names:
filename = osp.join(pose_dir, 'camera_pose_{:s}.txt'.format(name))
T = np.eye(4)
with open(filename, 'r') as f:
f.readline()
T[0, 3] = float(f.readline().strip())
T[1, 3] = float(f.readline().strip())
T[2, 3] = float(f.readline().strip())
f.readline()
f.readline()
T[0, :3] = [float(v) for v in f.readline().strip().split()]
T[1, :3] = [float(v) for v in f.readline().strip().split()]
T[2, :3] = [float(v) for v in f.readline().strip().split()]
Ts.append(np.linalg.inv(T))
rgb_im_dir = osp.join(data_dir, 'thermal_images')
im = cv2.imread(osp.join(rgb_im_dir, '{:s}.png'.format(names[0])))
im_shape = im.shape
# camera intrinsic
cinfo_filename = cinfo_manager.getPackageFileName(
'package://contactdb_utils/calibrations/boson.yaml')
cinfo = cinfo_manager.loadCalibrationFile(cinfo_filename, 'boson')
h_scaling = float(im_shape[0]) / cinfo.height
w_scaling = float(im_shape[1]) / cinfo.width
K = np.asarray(cinfo.K)
K[0] *= w_scaling
K[2] *= w_scaling
K[4] *= h_scaling
K[5] *= h_scaling
K = K.reshape((3, 3))
intrinsic = open3d.PinholeCameraIntrinsic(im_shape[1], im_shape[0],
K[0, 0], K[1, 1], K[0, 2], K[1,
2])
# render depth maps
depth_ims = render_depth_maps(mesh_filename, intrinsic, Ts)
# save masks
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,
(dilate_size, dilate_size))
for name, depth_im in zip(names, depth_ims):
mask_filename = osp.join(data_dir, 'thermal_images',
'{:s}_mask.png'.format(name))
mask = np.uint8(255 * (depth_im > 0))
mask = cv2.dilate(mask, kernel)
if cv2.imwrite(mask_filename, mask.astype(np.uint8)):
logger.info('Written {:s}'.format(mask_filename))