plt.figure(3)
base_T_cam = ptrans.transform_from(base_T_cam_R, base_T_cam_t, True)
tm = TransformManager()
tm.add_transform("base", "cam", base_T_cam)
axis = tm.plot_frames_in("base", s=0.3)
plt.show()
if __name__ == '__main__':
ROOT = os.path.dirname(os.path.abspath(__file__))
sys.path.append(ROOT)
arm = FrankaController(ROOT + '/config/franka.yaml')
cam = realsense(frame_width=1280, frame_height=720, fps=30)
cfg = read_cali_cfg("config/calibration.yaml")
initial_pose = cfg['initial_position']
cube_size = cfg['sample_cube_size']
maker_size = cfg['marker_size']
sample_repeat_num = cfg['marker_repeat_sample']
x_step = cfg['x_stride']
y_step = cfg['y_stride']
z_step = cfg['z_stride']
rotation_upper_limit = cfg['rotation_upper_limit']
rotation_lower_limit = cfg['rotation_lower_limit']
# Bounding box color in BGR
bbox_color = (255, 0, 0)
text_color = (255, 255, 255)
thickness = 2
cv.rectangle(img, rect, bbox_color, thickness)
cv.putText(img, text, (rect[0], rect[1]), cv.FONT_HERSHEY_PLAIN, 1.2, text_color)
return img
if __name__ == '__main__':
cfg = read_cfg('config/test_yolov5.yaml')
cam = realsense(frame_width = cfg['width'], frame_height = cfg['height'], fps = cfg['fps'])
engine_path = cfg['engine_path']
input_w = cfg['input_width']
input_h = cfg['input_height']
yolov5_module.init_inference(engine_path)
stored_exception = None
while True:
try:
if stored_exception:
break
_, color_img = cam.get_frame_cv()
#cv2.imshow('original', img)
#cv2.imshow('crop', crop)
# cv2.imshow('Gray image', gray)
# cv2.imshow('Blur', blur)
#cv2.imshow('binary', binary)
#cv2.imshow('bbox', img)
#cv2.waitKey(10)
return x, y, w, h
if __name__ == '__main__':
cam = realsense()
while (True):
_, img = cam.get_frame_cv()
crop_offset_col = 300
crop_offset_row = 100
crop = img[crop_offset_row:, crop_offset_col:-60]
gray = cv2.cvtColor(crop, cv2.COLOR_BGR2GRAY)
blur = cv2.blur(gray, (4, 4))
_, binary = cv2.threshold(blur, 200, 256, cv2.THRESH_BINARY)
# cntrs, _ = cv2.findContours(binary, 1, 2)
# cnt = cntrs[0]
