def publish_extrinsic(self, cam_label, extrinsic_data):
""" Publish extrinsic result
Args:
cam_label: `String` camera label of extrinsic calibration
extrinsic_data: `Extrinsic` extrinsic calibration data to send
in message
Returns:
"""
try:
extrinsic_msg = Extrinsic()
extrinsic_msg.cam_label = cam_label
extrinsic_msg.projection_matrix = flat_matrix_for_service(
numpy_array=extrinsic_data["M"])
extrinsic_msg.vp = extrinsic_data["vp"]
extrinsic_msg.p1 = extrinsic_data["p1"]
extrinsic_msg.p2 = extrinsic_data["p2"]
extrinsic_msg.p3 = extrinsic_data["p3"]
extrinsic_msg.p4 = extrinsic_data["p4"]
extrinsic_msg.dead_view = extrinsic_data["dead_view"]
extrinsic_msg.ppmx = extrinsic_data["ppmx"]
extrinsic_msg.ppmy = extrinsic_data["ppmy"]
extrinsic_msg.unwarped_size = extrinsic_data["unwarped_size"]
extrinsic_msg.image_size = extrinsic_data["image_size"]
self.pb_calibrator_extrinsic.publish(extrinsic_msg)
except Exception as e:
printlog(msg="Error publishing extrinsic calibration"
"trought topic, {}".format(e),
msg_type="ERROR")
return False
def cb_extrinsic_params(self, msg):
""" Re-assing extrinsic calibration
Args:
msg: `VisualMessage` message for visual debugger
data: `string` message content
type: `string` message type
Returns:
"""
try:
self.extrinsic.M = msg.projection_matrix
self.extrinsic.p1 = msg.p1
self.extrinsic.p2 = msg.p2
self.extrinsic.p3 = msg.p3
self.extrinsic.p4 = msg.p4
self.extrinsic.vp = msg.vp
self.extrinsic.dead_view = msg.dead_view
self.extrinsic.ppmx = msg.ppmx
self.extrinsic.ppmy = msg.ppmy
self.extrinsic.warped_size = msg.unwarped_size
self.extrinsic.image_size = msg.image_size
except Exception as e:
printlog(msg="Error getting extrinsic calibration"
"from topic, {}".format(e),
msg_type="ERROR")
return False
printlog(msg="Extrincid parameters updated", msg_type="INFO")
# =============================================================================
def run(self):
""" Cycle of threadh execution
Args:
Returns:
"""
while True:
self.tick = time.time()
try:
# read images from cameras
imgs_dic = dict(
map(lambda o: (o.cam_label, o.get_image()),
self.cams_caps.camera_handlers.values()))
# Show calibration if something to show
if self.calibrator_img is not None:
imgs_dic["P"] = self.calibrator_img
else: # Else user grapich components
imgs_dic["P"] = imgs_dic["C"].copy()
self.draw_components(imgs_dict=imgs_dic)
# Optimize image
if self._FR_STREAMING_OPTIMIZER:
self.img_optimizer.optimize(img=imgs_dic["P"])
# TODO: integrate stitcher
# if self._STITCHER and "other_condition":
# self.img_stitch(imgs_dic)
# -------------------------------------------------------------
# Publish image
if self.pub_streaming is not None:
img_msg = self.img_bridge.cv2_to_imgmsg(cvim=imgs_dic["P"],
encoding="bgr8")
t = str(self.get_clock().now().nanoseconds)
img_msg.header.stamp.sec = int(t[0:10])
img_msg.header.stamp.nanosec = int(t[10:])
img_msg.header.frame_id = t
self.pub_streaming.publish(img_msg)
# -------------------------------------------------------------
# Operate times for next frame iteration
tock = time.time() - self.tick
twait = 1. / self.cams_config["C"]["FPS"] - tock
if twait <= 0.:
continue
time.sleep(twait)
# print("fps:", 1./(time.time() - self.tick), flush=True)
except Exception as e:
printlog(msg=e, msg_type="ERROR")
def run(self):
"""
Run cicle of threadh
Args:
returns:
"""
while True:
try:
self.draw_mouse_event(img=self.streaming_img)
cv2.imshow(self.win_name, self.streaming_img)
key = cv2.waitKey(self.win_time)
self.cb_key_event(key=key)
except Exception as e:
printlog(msg=e, msg_type="ERROR")
def cb_streaming_img(self, data):
"""
When called the variable self.streaming_img will be assigned
with the incoming data
Args:
data: `sensor_msgs.msg/Image` image
Returns:
"""
try:
self.streaming_img = self.img_bridge.imgmsg_to_cv2(
img_msg=data, desired_encoding="bgr8")
except CvBridgeError as e:
printlog(msg="subscriber for camera in topic {}, {}".format(
self.streaming_topic, e),
log_type="ERROR")
def cb_img_to_calibrate(self, msg):
""" Callback function to update camera image to calibrate
Args:
data: `cv2.math` image to calibrate
Returns:
"""
try:
self.cam_img = self.img_bridge.imgmsg_to_cv2(
img_msg=msg, desired_encoding="bgr8")
except CvBridgeError as e:
printlog(
msg=
"erro while getting data from video_calibrator/calibrate_img, {}"
.format(e),
msg_type="ERROR")
def pub_log(self, msg, msg_type="INFO", prompt=True):
"""
Print log message tracking file and function caller
Args:
msg: `string` message to print
msg_type: `string` message type
prompt: `boolean` sure that any output is buffered and go to the
destination.
Returns:
"""
self.pb_visual_debugger_msg.data = msg
self.pb_visual_debugger_msg.type = msg_type
self.pb_visual_debugger.publish(self.pb_visual_debugger_msg)
if prompt:
printlog(msg=self.pb_visual_debugger_msg.data, msg_type=msg_type)
def cb_cal_img_result(self, msg):
""" Callback function to assing image calibraion result and show it
on main supervisors image
Args:
Returns:
"""
try:
self.calibrator_img = self.img_bridge.imgmsg_to_cv2(
img_msg=msg, desired_encoding="bgr8")
except CvBridgeError as e:
printlog(msg="erro while getting data from video_calibrator/"
"calibrate_img_result, {}".format(e),
msg_type="ERROR")
time.sleep(int(os.getenv(key="VISION_CAL_SHOW_TIME", default=5)))
self.calibrator_img = None
def save_extrinsic(self, file_path, data):
""" Saves file with extrinsic calibration
Args:
file_path: `String` name to save mono vision parameters file
data: `Dictionary` with extrinsic calibration to save in file
Returns:
"""
try: # Save the calibration data in file
with open(file_path, 'w') as outfile:
yaml.dump(data, outfile, default_flow_style=False)
printlog(
msg="Camera extrinsic calibration saved {}".format(file_path),
msg_type="INFO")
except IOError as e: # Report any error saving de data
printlog(
msg="Problem saving camera extrinsic calibration: {}".format(
e),
msg_type="ERROR")
def cb_key_event(self, key):
"""
Get and execute action from users key
Args:
key: 'int' keyboard actions
returns:
"""
# If pressed No key then continue
if key == -1:
pass
# If pressed TAB key then open show help/menu options
elif key == 9:
pass
# If pressed 1 Key then calibrate camera LL
elif key == 49:
self.pub_cam_calibrate_msg.data = "LL"
self.pub_cam_calibrate.publish(self.pub_cam_calibrate_msg)
# If pressed 2 Key then calibrate camera C
elif key == 50:
self.pub_cam_calibrate_msg.data = "C"
self.pub_cam_calibrate.publish(self.pub_cam_calibrate_msg)
# If pressed 3 Key then calibrate camera RR
elif key == 51:
self.pub_cam_calibrate_msg.data = "RR"
self.pub_cam_calibrate.publish(self.pub_cam_calibrate_msg)
# If pressed 4 key then calibrate camera B
elif key == 52:
self.pub_cam_calibrate_msg.data = "B"
self.pub_cam_calibrate.publish(self.pub_cam_calibrate_msg)
# If pressed right key then move to right
elif key == 81:
pass
# If pressed up key then move to forward
elif key == 82:
pass
# If pressed left key then move to left
elif key == 83:
pass
# If pressed down key then move to backwards
elif key == 84:
pass
# If pressed A key then switch to left camera
elif key == 97:
pass
# If pressed M key then stiwch bewteen manual and waypoint mode
elif key == 109:
pass
# If pressed D key then switch to right camera
elif key == 100:
pass
# If pressed P key then open the lid
elif key == 112:
pass
# If pressed Q key then quit
elif key == 113:
exit()
# If pressed R key then switch to rear camera
elif key == 114:
pass
# If pressed S key then activate/desactivate stitching mode
elif key == 115:
pass
# If pressed no key defined then print message
else:
printlog(msg=f"{key} key action no defined", msg_type="WARN")
def __init__(self):
"""
Object class constructor
Args:
Returns:
"""
# ---------------------------------------------------------------------
super().__init__('MappingNode')
# Allow callbacks to be executed in parallel without restriction.
self.callback_group = ReentrantCallbackGroup()
Thread.__init__(self)
GraphicInterface.__init__(self, parent_node=self)
# ---------------------------------------------------------------------
self._LOCAL_RUN = int(os.getenv(key="LOCAL_LAUNCH", default=0))
self._LOCAL_GUI = int(os.getenv(key="LOCAL_GUI", default=0))
self._CONF_PATH = str(
os.getenv(key="CONF_PATH",
default=os.path.dirname(os.path.abspath(__file__))))
self._FR_AGENT = int(os.getenv(key="FR_AGENT", default=0))
self._FR_STREAMING_OPTIMIZER = int(
os.getenv(key="FR_STREAMING_OPTIMIZER", default=0))
self._VIDEO_WIDTH = int(os.getenv(key="VIDEO_WIDTH", default=640))
self._VIDEO_HEIGHT = int(os.getenv(key="VIDEO_HEIGHT", default=360))
self._STITCHER = int(os.getenv(key="STITCHER", default=0))
self._STITCHER_SUP_MODE = int(
os.getenv(key="STITCHER_SUP_MODE", default=0))
# ---------------------------------------------------------------------
# Initiate CameraSupervisors Class that handles the threads that reads
# the cameras
self.cams_config = read_cams_configuration(
FILE_NAME="cams_conf_local.yaml"
if self._LOCAL_RUN else "cams_conf.yaml",
CONF_PATH=self._CONF_PATH)
if self.cams_config is None: # Validate cameras status
self.get_logger().error(
"No cameras were configured in configuration")
exit()
else:
self.get_logger().info("cameras configuration loaded")
# Start cameras handler with configuration
self.cams_caps = CamerasCaptures(cams_config=self.cams_config)
# print(cams_status = self.cams_caps.get_cameras_status())
self.img_optimizer = StreamingOptimizer()
self.img_bridge = CvBridge()
# ---------------------------------------------------------------------
# Stitcher object
self.stitcher = None
if self._STITCHER:
self.stitcher = Stitcher(abs_path=os.path.join(
self._CONF_PATH, "stitcher_config.npz"),
super_stitcher=self._STITCHER_SUP_MODE
) if self._STITCHER else None
# ---------------------------------------------------------------------
# Services
# ---------------------------------------------------------------------
# Publishers
# Cameras streaming images
self.pub_streaming = None
if self._FR_AGENT and "C" in self.cams_config.keys():
if self.cams_config["C"]["TOPIC"] != "None":
self.pub_streaming = self.create_publisher(
Image, self.cams_config["C"]["TOPIC"], 10)
else:
printlog(msg="No topic for video camera C streaming topic"
", no thread and topic will be created",
msg_type="ERROR")
# Cameras status
self.pb_cams_status = self.create_publisher(
CamerasStatus,
'video_streaming/cams_status',
5,
callback_group=self.callback_group)
# Image to calibrate
self.pb_img_to_calibrate = self.create_publisher(
Image,
'video_calibrator/calibrate_img',
5,
callback_group=self.callback_group)
self.sub_calibration = self.create_subscription(
msg_type=String,
topic='video_calibrator/calibrate_cam',
callback=self.cb_send_img_calibrate,
qos_profile=5,
callback_group=self.callback_group)
# ---------------------------------------------------------------------
# Subscribers
self.calibrator_img = None
self.sub_calibrator_img_res = self.create_subscription(
msg_type=Image,
topic='video_calibrator/calibrate_img_result',
callback=self.cb_cal_img_result,
qos_profile=5,
callback_group=self.callback_group)
# ---------------------------------------------------------------------
self.tm_pub_cams_status = self.create_timer(1.0, self.cb_cams_status)
# ---------------------------------------------------------------------
# Thread variables
self.run_event = Event()
self.run_event.set()
self.tick = time.time()
# self.daemon = True
self.start()
def load(self):
"""
Loads intrinsic camera parameters from file
Args:
Returns:
"""
try:
abs_path = os.path.join(self.file_path, self.file_name)
if os.path.isfile(abs_path):
with open(abs_path, 'r') as stream:
data = yaml.safe_load(stream)
else:
printlog(msg="No instrinsic configuration file {}".format(
self.file_name),
msg_type="ERROR")
return
for key in [
"camera_matrix", "distortion_coefficients",
"rectification_matrix", "projection_matrix"
]:
if key not in data:
printlog(
msg="Intrinsic file {}, invalid".format(FILE_NAME),
msg_type="ERROR")
raise Exception('invalid file format')
data[key] = \
np.array(data[key]["data"]).reshape(
data[key]["rows"],
data[key]["cols"])
self.image_width = data["image_width"]
self.image_height = data["image_height"]
self.mtx = data["camera_matrix"]
self.distortion_model = data["distortion_model"]
self.distortion_coefficients = data["distortion_coefficients"]
self.rectification_matrix = data["rectification_matrix"]
self.projection_matrix = data["projection_matrix"]
map1, map2 = cv2.initUndistortRectifyMap(
cameraMatrix=self.mtx,
distCoeffs=self.distortion_coefficients,
R=np.array([]),
newCameraMatrix=self.mtx,
size=(data["image_width"], data["image_height"]),
m1type=cv2.CV_8UC1)
self.map1 = map1
self.map2 = map2
printlog(msg="{} instrinsic configuration loaded".format(
self.file_name),
msg_type="OKGREEN")
except Exception as e:
self.image_width = None
self.image_height = None
self.mtx = None
self.distortion_model = None
self.distortion_coefficients = None
self.rectification_matrix = None
self.projection_matrix = None
self.map1 = None
self.map2 = None
printlog(msg="instrinsic file {} error, {}".format(
self.file_name, e),
msg_type="ERROR")
def calibrate_distances(self, img_src, camera_label):
"""
Print log message tracking file and function caller
Args:
img_src: `cv2.math` image to calibrate
camera_label: `string` camera label
Returns:
"""
# Perform extrinsic calibration
ext_cal = find_projection(
img_src=img_src.copy(),
mtx=self.intrinsic.mtx,
dist=self.intrinsic.distortion_coefficients,
PATTERN_THRESH_TOP=self._VISION_CAL_PAT_TH_TOP,
PATTERN_THRESH_BOTTOM=self._VISION_CAL_PAT_TH_BOTTOM,
PATTERN_ITERATION_TRIES=self._VISION_CAL_PAT_ITE_TRIES,
HSVI=self._HSVI,
HSVS=self._HSVS,
LOCAL_CALIBRATION=self._LOCAL_RUN and self._VISION_CAL_SHOW_LOCAL)
if ext_cal is not None:
try:
# Calculate rotation matrix from surface
M = get_rot_matrix(p1=ext_cal["p1"],
p2=ext_cal["p2"],
p3=ext_cal["p3"],
p4=ext_cal["p4"],
UNWARPED_SIZE=self._UNWARPED_SIZE)
# Calculate the pixel relation in 'X' and 'Y' from surface
# projection and Rotation Matrix
surf_relation = pixel_relation(
img_src=img_src.copy(),
M=M,
mtx=self.intrinsic.mtx,
dist=self.intrinsic.distortion_coefficients,
p1=ext_cal["p1"],
p2=ext_cal["p2"],
p3=ext_cal["p3"],
p4=ext_cal["p4"],
Left_Line=ext_cal["Left_Line"],
Right_Line=ext_cal["Right_Line"],
Hy=ext_cal["Hy"],
Dx_lines=self._VISION_CAL_PAT_HOZ,
Dy_lines=self._VISION_CAL_PAT_VER,
UNWARPED_SIZE=self._UNWARPED_SIZE,
HSVI=self._HSVI,
HSVS=self._HSVS,
PATTERN_ITERATION_TRIES=self._VISION_CAL_PAT_ITE_TRIES)
if not surf_relation:
raise RuntimeError("No pattern found correctly")
return False
# Save mono vision camera measurement parameters in file
extrinsic_file_name = 'Extrinsic_{}_{}_{}.yaml'.format(
self._VIDEO_WIDTH, self._VIDEO_HEIGHT, camera_label)
extrinsic_abs_path = os.path.join(self._CONF_PATH,
extrinsic_file_name)
extrinsic_vision_params = {
"M": M.tolist(),
"vp": [int(ext_cal["vp"][0]),
int(ext_cal["vp"][1])],
"p1": list(ext_cal["p1"]),
"p2": list(ext_cal["p2"]),
"p3": list(ext_cal["p3"]),
"p4": list(ext_cal["p4"]),
"dead_view": surf_relation["dead_view_distance"],
"ppmx": surf_relation["ppmx"],
"ppmy": surf_relation["ppmy"],
"unwarped_size": list(self._UNWARPED_SIZE),
"image_size": list(self._VIDEO_SIZE)
}
self.save_extrinsic(file_path=extrinsic_abs_path,
data=extrinsic_vision_params)
# Set calibration image and show it to pilots console
mask_pro, mask_src = create_ruler_mask(
flag_img=self._flag_image,
extrinsic_params=extrinsic_vision_params)
self.calibration_image = overlay_image(l_img=img_src.copy(),
s_img=mask_src,
pos=(0, 0),
transparency=1)
calibration_image = self.calibration_image.copy()
# overlay with rules in wrapped perspective
cal_img_pro = cv2.warpPerspective(src=self.calibration_image,
M=M,
dsize=self._UNWARPED_SIZE)
ruler_projected_orig = overlay_image(l_img=cal_img_pro,
s_img=mask_pro,
pos=(0, 0),
transparency=1)
ruler_projected = cv2.resize(src=cv2.rotate(
src=ruler_projected_orig,
rotateCode=cv2.ROTATE_90_CLOCKWISE),
dsize=self._VIDEO_SIZE)
if self._LOCAL_RUN:
cv2.imwrite(
os.path.join(
os.path.dirname(os.path.realpath(__file__)),
"extrinsic", "extrinsic_cal_projection.jpg"),
ruler_projected)
cv2.imwrite(
os.path.join(
os.path.dirname(os.path.realpath(__file__)),
"extrinsic", "extrinsic_cal_source.jpg"),
calibration_image)
# Send supervisor's image message to topic
try:
img_ext_result = self.img_bridge.cv2_to_imgmsg(
cvim=ruler_projected, encoding="bgr8")
# img_msg.header.stamp = time.time()
self.pb_calibrator_result.publish(img_ext_result)
time.sleep(self._VISION_CAL_SHOW_TIME)
img_ext_result = self.img_bridge.cv2_to_imgmsg(
cvim=calibration_image, encoding="bgr8")
# img_msg.header.stamp = time.time()
self.pb_calibrator_result.publish(img_ext_result)
time.sleep(self._VISION_CAL_SHOW_TIME)
except CvBridgeError as e:
self.get_logger().error(
"publishing extrinsic result images through topic, {}".
format(e))
# Publish extrinsic result trought topic
self.publish_extrinsic(cam_label=camera_label,
extrinsic_data=extrinsic_vision_params)
# Report successfully calibration
self.pub_log(
msg="Calibracion para la camara {} exitosa!".format(
camera_label),
msg_type="OKGREEN")
return True
except Exception as e:
printlog(
msg="No se pudo encontrar el patron de calibracion! ({})".
format(e),
msg_type="ERROR")
return False
else:
self.pub_log(msg="No se pudo encontrar el patron de calibracion!",
msg_type="ERROR")
return False
def cb_calibrate(self, msg):
""" Callback function to calibration a camera
Args:
data: `String` ross message with camera label to calibrate
Returns:
"""
# ---------------------------------------------------------------------
cam_label = msg.data
self.pub_log(msg="Calibrating camera {} ...".format(cam_label))
self.calibrating_stat = True
# ---------------------------------------------------------------------
# Check if camera label exits
if cam_label not in self.extrinsic.keys():
printlog(msg="{} is not a camera label available in extrinsics {}".
format(cam_label, self.extrinsic.keys()),
msg_type="ERROR")
time.sleep(10)
return
# If runing on local launch load a default picture to test calibration
if self._LOCAL_RUN:
self.cam_img = cv2.imread(
os.path.join(os.path.dirname(os.path.realpath(__file__)),
"extrinsic/calibration_default.jpg"),
int(cv2.IMREAD_UNCHANGED))
else:
# wait image from topic, for now with simulation image
timeout = time.time() + 10 # 10 seconds from now
while True:
if self.cam_img is not None or time.time() > timeout:
break
if self.cam_img is None:
printlog(
msg=
"Timeout getting image from topic, camera no calibrated",
msg_type="ERROR")
self.calibrating_stat = False
return False
if self.intrinsic.mtx is not None and not self._LOCAL_RUN:
# Resize image if size diferento to intrinsic calibration size
if (self.cam_img.shape[1] != self.intrinsic.image_width and
self.cam_img.shape[0] != self.intrinsic.image_height):
self.cam_img = c2.resize(self.cam_img,
(self.intrinsic.image_width,
self.intrinsic.image_height))
# Undistord image if intrinsic calibration
self.cam_img = cv2.remap(self.cam_img, self.intrinsic.map1,
self.intrinsic.map2, cv2.INTER_LINEAR)
try:
# If exits continue calibration process
if self.intrinsic.mtx is not None:
self.calibrate_distances(img_src=self.cam_img,
camera_label=cam_label)
else:
self.pub_log(
msg="No intrinsic calibration to perform extrinsic",
msg_type="ERROR")
except OSError as err:
printlog(msg="OS error: {0}".format(err), msg_type="ERROR")
except ValueError:
printlog(msg="ValueError raised", msg_type="ERROR")
except Exception as e:
printlog(msg="Exception error: {}".format(e), msg_type="ERROR")
except:
printlog(msg="Unexpected error: {}".format(sys.exc_info()[0]),
msg_type="ERROR")
raise
self.calibrating_stat = False
self.cam_img = None
