def __init__(self, input_port_name, output_port_name, display_port_name):
yarp.RFModule.__init__(self)
# Prepare ports
self._input_port = yarp.Port()
self._input_port_name = input_port_name
self._input_port.open(self._input_port_name)
self._output_port = yarp.BufferedPortBottle()
self._output_port_name = output_port_name
self._output_port.open(self._output_port_name)
self._display_port = yarp.Port()
self._display_port_name = display_port_name
self._display_port.open(self._display_port_name)
# Prepare image buffers
# Input
self._input_buf_image = yarp.ImageRgb()
self._input_buf_image.resize(320, 240)
self._input_buf_array = np.zeros((240, 320, 3), dtype=np.uint8)
self._input_buf_image.setExternal(self._input_buf_array,
self._input_buf_array.shape[1], self._input_buf_array.shape[0])
# Output
self._display_buf_image = yarp.ImageRgb()
self._display_buf_image.resize(320, 240)
self._display_buf_array = np.zeros((240, 320, 3), dtype=np.uint8)
self._display_buf_image.setExternal(self._display_buf_array,
self._display_buf_array.shape[1], self._display_buf_array.shape[0])
self._logger = yarp.Log()
def createImageArrays(self):
self.imageArray = numpy.zeros((self.imgHeight, self.imgWidth, 3),
dtype=numpy.uint8)
self.newImage = yarp.ImageRgb()
self.yarpImage = yarp.ImageRgb()
self.yarpImage.resize(self.imgWidthNew, self.imgWidthNew)
self.yarpImage.setExternal(self.imageArray, self.imageArray.shape[1],
self.imageArray.shape[0])
def processLiveData(self,
dataList,
thisModel,
verbose,
additionalData=dict()):
"""
Method which receives a list of data frames and outputs a classification if available or 'no_classification' if it is not
Args:
dataList: List of dataFrames collected. Length of list is variable.
thisModel: List of models required for testing.
verbose : Boolean turning logging to stdout on or off.
additionalData : Dictionary containing additional data required for classification to occur.
Returns:
String with result of classification, likelihood of the classification, and list of frames with the latest x number of frames popped where x is the window length of the model. Classification result can be string `'None'` if the classification is unknown or message is invalid or `None` if a different error occurs.
"""
logging.info('process live data')
logging.info(len(dataList))
imgH = thisModel[0].paramsDict['imgH']
imgW = thisModel[0].paramsDict['imgW']
imgHNew = thisModel[0].paramsDict['imgHNew']
imgWNew = thisModel[0].paramsDict['imgWNew']
numFaces = len(dataList)
imageArray = numpy.zeros((imgH, imgW, 3), dtype=numpy.uint8)
yarpImage = yarp.ImageRgb()
yarpImage.resize(imgH, imgW)
yarpImage.setExternal(imageArray, imageArray.shape[1],
imageArray.shape[0])
# images = numpy.zeros((numFaces, imgHNew * imgWNew), dtype=numpy.uint8)
labels = [None] * numFaces
likelihoods = [None] * numFaces
if numFaces > 0:
# average all faces
for i in range(numFaces):
logging.info('iterating' + str(i))
yarpImage.copy(dataList[i])
imageArrayOld = cv2.resize(imageArray, (imgHNew, imgWNew))
imageArrayGray = cv2.cvtColor(imageArrayOld,
cv2.COLOR_BGR2GRAY)
instance = imageArrayGray.flatten()[None, :]
logging.info(instance.shape)
logging.info("Collected face: " + str(i))
logging.info('testing enter')
[labels[i], likelihoods[i]
] = SAMTesting.testSegment(thisModel, instance, verbose, None)
logging.info('testing leave')
logging.info('combine enter')
finalClassLabel, finalClassProb = SAMTesting.combineClassifications(
thisModel, labels, likelihoods)
logging.info('combine ready')
logging.info('finalClassLabels ' + str(finalClassLabel))
logging.info('finalClassProbs ' + str(finalClassProb))
return finalClassLabel, finalClassProb, []
else:
return [None, 0, None]
def __init__(self):
yarp.RFModule.__init__(self)
# handle port for the RFModule
self.handle_port = yarp.Port()
self.attach(self.handle_port)
# Define vars to receive an image
self.input_port = yarp.BufferedPortImageRgb()
# Create numpy array to receive the image and the YARP image wrapped around it
self.input_img_array = None
# Define vars for outputing image
self.output_objects_port = yarp.Port()
self.output_raw_port = yarp.Port()
self.output_img_port = yarp.Port()
self.display_buf_array = None
self.display_buf_image = yarp.ImageRgb()
self.module_name = None
self.width_img = None
self.height_img = None
self.label_path = None
self.category_index = None
self.detection_graph = None
self.model_path = None
self.cap = None
self.session = None
def __init__(self, width=640, height=480):
# Initialise YARP
yarp.Network.init()
# create ros node
rospy.init_node('icub_images_driver', anonymous=True)
# create the publishers
self.image_pub = rospy.Publisher('/icubRos/sensors/camera_left', Image, queue_size=10)
# Create a port and connect it to the iCub simulator virtual camera
self.input_port_cam = yarp.Port()
self.input_port_cam.open("/icubRos/camera_left")
yarp.Network.connect("/icubSim/cam/left", "/icubRos/camera_left")
# prepare image
self.yarp_img_in = yarp.ImageRgb()
self.yarp_img_in.resize(width,height)
self.img_array = np.zeros((height, width, 3), dtype=np.uint8)
# yarp image will be available in self.img_array
self.yarp_img_in.setExternal(self.img_array, width, height)
# create time variable for calculating reading time
self.current_time = time.time()
print ("Starting reading")
rate = rospy.Rate(10)
rate = 10.0
while not rospy.is_shutdown():
#while True:
self.read_and_publish_in_ros()
#rate.sleep()
time.sleep(1/rate)
def readFrame(self):
"""
Logic to read an available data frame.
Description:
This function first checks the required data type of the frame to be received and instantiates the required yarp data object. This function then subsequently reads in the latest frame from the __dataIn__ port which it returns. Return is `None` if the data type is not recognised. This is currently a limitation because `ImageRgb`, `ImageMono` and `Bottle` are so far the only supported bottle types. This can be easily extended in this section by adding more cases.
Returns: Boolean indicating success of logic or not.
"""
if self.inputType == 'imagergb':
frame = yarp.ImageRgb()
elif self.inputType == 'imagemono':
frame = yarp.ImageMono()
elif self.inputType == 'bottle':
frame = yarp.Bottle()
else:
return None
frameRead = self.portsList[self.labelPort].read(True)
if self.inputType == 'bottle':
frame.fromString(frameRead.toString())
elif 'image' in self.inputType:
frame.copy(frameRead)
return frame
def write_yarp_image(outport, img_array):
# Create the yarp image and wrap it around the array
# img_array = img_array[:, :, (2, 1, 0)]
yarp_img = yarp.ImageRgb()
yarp_img.resize(320, 240)
yarp_img.setExternal(img_array, img_array.shape[1], img_array.shape[0])
outport.write(yarp_img)
def createImageBuffer(width=320, height=240, channels=3):
""" This method creates image buffers with the specified \a width, \a height and number of
color channels \a channels.
@param width - integer specifying the width of the image (default: 320)
@param height - integer specifying the height of the image (default: 240)
@param channels - integer specifying number of color channels (default: 3)
@return image, buffer array
"""
if channels == 1:
buf_image = yarp.ImageFloat()
buf_image.resize(width, height)
buf_array = np.zeros((height, width), dtype=np.float32)
else:
buf_image = yarp.ImageRgb()
buf_image.resize(width, height)
buf_array = np.zeros((height, width, channels), dtype=np.uint8)
buf_image.setExternal(buf_array, buf_array.shape[1],
buf_array.shape[0])
return buf_image, buf_array
def __init__(self, input_port_name, output_port_name):
# Prepare ports
self._input_port = yarp.Port()
self._input_port_name = input_port_name
self._input_port.open(self._input_port_name)
self._output_port = yarp.Port()
self._output_port_name = output_port_name
self._output_port.open(self._output_port_name)
# Prepare image buffers
# Input
self._input_buf_image = yarp.ImageRgb()
self._input_buf_image.resize(320, 240)
self._input_buf_array = numpy.zeros((240, 320, 3), dtype=numpy.uint8)
self._input_buf_image.setExternal(
self._input_buf_array.__array_interface__['data'][0],
self._input_buf_array.shape[1], self._input_buf_array.shape[0])
# Output
self._output_buf_image = yarp.ImageFloat()
self._output_buf_image.resize(320, 240)
self._output_buf_array = numpy.zeros((240, 320), dtype=numpy.float32)
self._output_buf_image.setExternal(
self._output_buf_array.__array_interface__['data'][0],
self._output_buf_array.shape[1], self._output_buf_array.shape[0])
def __init_scene_camera(self) -> Tuple:
""" @returns the img array and yarp image"""
scene_img_array: np.ndarray = np.ones((240, 320, 3), np.uint8)
scene_yarp_image: yarp.ImageRgb = yarp.ImageRgb()
scene_yarp_image.resize(320, 240)
# YARP image will wrap the arr
scene_yarp_image.setExternal(scene_img_array.data,
scene_img_array.shape[1],
scene_img_array.shape[0])
return scene_img_array, scene_yarp_image
def write_yarp_image(self, frame):
"""
Handle function to stream the recognize faces with their bounding rectangles
:param img_array:
:return:
"""
self.display_buf_image = yarp.ImageRgb()
self.display_buf_image.resize(self.width_img, self.height_img)
self.display_buf_image.setExternal(frame.tobytes(), self.width_img,
self.height_img)
self.output_img_port.write(self.display_buf_image)
def read_yarp_image(inport):
# Create numpy array to receive the image and the YARP image wrapped around it
img_array = np.ones((240, 320, 3), dtype=np.uint8)
yarp_image = yarp.ImageRgb()
yarp_image.resize(320, 240)
yarp_image.setExternal(img_array, img_array.shape[1], img_array.shape[0])
# Read the data from the port into the image
inport.read(yarp_image)
return img_array, yarp_image
def readFrame(self):
if self.inputType == 'imagergb':
frame = yarp.ImageRgb()
elif self.inputType == 'imagemono':
frame = yarp.ImageMono()
elif self.inputType == 'bottle':
frame = yarp.Bottle()
frame = self.portsList[self.labelPort].read(True)
return frame
def __init__(self, options):
self.options = options
# Set requested GPU
os.environ["CUDA_DEVICE_ORDER"] = "PCI_BUS_ID"
os.environ["CUDA_VISIBLE_DEVICES"] = str(options.gpu_id)
# Initialize YARP network
yarp.Network.init()
# Initialize RF module
yarp.RFModule.__init__(self)
# Initialize inference
self.inference = Inference(self.options.model, self.options.refine_model, self.options.width, self.options.height, self.options.cam_fx, self.options.cam_fy, self.options.cam_cx, self.options.cam_cy)
# Initialize YARP ports
self.rgb_in = yarp.BufferedPortImageRgb()
self.rgb_in.open("/dense-fusion/camera/rgb:i")
self.depth_in = yarp.BufferedPortImageFloat()
self.depth_in.open("/dense-fusion/camera/depth:i")
self.mask_in = yarp.BufferedPortImageMono()
self.mask_in.open("/dense-fusion/camera/mask:i")
self.camera_pose_in = yarp.BufferedPortVector()
self.camera_pose_in.open("/dense-fusion/camera/pose:i")
self.prediction_out = yarp.Port()
self.prediction_out.open("/dense-fusion/pose:o")
# Input buffers initialization
self.rgb_buffer = bytearray(numpy.zeros((self.options.height, self.options.width, 3), dtype = numpy.uint8))
self.rgb_image = yarp.ImageRgb()
self.rgb_image.resize(self.options.width, self.options.height)
self.rgb_image.setExternal(self.rgb_buffer, self.options.width, self.options.height)
self.depth_buffer = bytearray(numpy.zeros((self.options.height, self.options.width, 1), dtype = numpy.float32))
self.depth_image = yarp.ImageFloat()
self.depth_image.resize(self.options.width, self.options.height)
self.depth_image.setExternal(self.depth_buffer, self.options.width, self.options.height)
self.mask_buffer = bytearray(numpy.zeros((self.options.height, self.options.width, 1), dtype = numpy.uint8))
self.mask_image = yarp.ImageMono()
self.mask_image.resize(self.options.width, self.options.height)
self.mask_image.setExternal(self.mask_buffer, self.options.width, self.options.height)
# Stamp initialization for finite differences
self.last_stamp = -1
self.last_total_position = None
def write_template_face(self, template_img):
"""
Handle function to stream the tempalte of the recognize face
:param img_array:
:return:
"""
self.display_buf_image = yarp.ImageRgb()
self.display_buf_image.resize(template_img.shape[1],
template_img.shape[0])
self.display_buf_image.setExternal(template_img.tobytes(),
template_img.shape[1],
template_img.shape[0])
self.face_template.write(self.display_buf_image)
def __initilizeBinocularImages(
self
) -> Tuple[yarp.ImageRgb, np.ndarray, yarp.ImageRgb, np.ndarray]:
""" Create numpy array to receive the image and the YARP image wrapped around it """
# print('------------- Init image-array structures -----------------')
# YARP images for both eyes and the arr
left_eye_img_array: np.ndarray = np.ones((240, 320, 3), np.uint8)
left_eye_yarp_image: yarp.ImageRgb = yarp.ImageRgb()
left_eye_yarp_image.resize(320, 240)
right_eye_img_array: np.ndarray = np.ones((240, 320, 3), np.uint8)
right_eye_yarp_image: yarp.ImageRgb = yarp.ImageRgb()
right_eye_yarp_image.resize(320, 240)
# YARP image will wrap the arr
left_eye_yarp_image.setExternal(left_eye_img_array.data,
left_eye_img_array.shape[1],
left_eye_img_array.shape[0])
right_eye_yarp_image.setExternal(right_eye_img_array.data,
right_eye_img_array.shape[1],
right_eye_img_array.shape[0])
return right_eye_yarp_image, right_eye_img_array, left_eye_yarp_image, left_eye_img_array
def processLiveData(self,
dataList,
thisModel,
verbose,
additionalData=dict()):
logging.info('process live data')
logging.info(len(dataList))
imgH = thisModel[0].paramsDict['imgH']
imgW = thisModel[0].paramsDict['imgW']
imgHNew = thisModel[0].paramsDict['imgHNew']
imgWNew = thisModel[0].paramsDict['imgWNew']
numFaces = len(dataList)
imageArray = numpy.zeros((imgH, imgW, 3), dtype=numpy.uint8)
yarpImage = yarp.ImageRgb()
yarpImage.resize(imgH, imgW)
yarpImage.setExternal(imageArray, imageArray.shape[1],
imageArray.shape[0])
# images = numpy.zeros((numFaces, imgHNew * imgWNew), dtype=numpy.uint8)
labels = [None] * numFaces
likelihoods = [None] * numFaces
if numFaces > 0:
# average all faces
for i in range(numFaces):
logging.info('iterating' + str(i))
yarpImage.copy(dataList[i])
imageArrayOld = cv2.resize(imageArray, (imgHNew, imgWNew))
imageArrayGray = cv2.cvtColor(imageArrayOld,
cv2.COLOR_BGR2GRAY)
instance = imageArrayGray.flatten()[None, :]
logging.info(instance.shape)
logging.info("Collected face: " + str(i))
logging.info('testing enter')
[labels[i], likelihoods[i]
] = SAMTesting.testSegment(thisModel, instance, verbose, None)
logging.info('testing leave')
logging.info('combine enter')
finalClassLabel, finalClassProb = SAMTesting.combineClassifications(
thisModel, labels, likelihoods)
logging.info('combine ready')
logging.info('finalClassLabels ' + str(finalClassLabel))
logging.info('finalClassProbs ' + str(finalClassProb))
return finalClassLabel, finalClassProb, []
else:
return [None, 0, None]
def configure(self, rf):
yarp.Network.init()
self.respondPort = yarp.Port()
self.inputPort = yarp.BufferedPortImageRgb()
self.outputStillPort = yarp.BufferedPortImageRgb()
self.outputImagePort = yarp.BufferedPortImageRgb()
self.respondPort.open('/imageRPC')
self.inputPort.open('/pythonRead')
self.outputImagePort.open('/imageWrite')
self.outputStillPort.open('/pictureWrite')
self.attach(self.respondPort)
yarp.Network.connect('/grabber', '/pythonRead')
yarp.Network.connect('/pictureWrite', '/pictureView')
yarp.Network.connect('/imageWrite', '/internalView')
self.imgArray = numpy.zeros((240, 320, 3), dtype=numpy.uint8)
self.imgBuffer = numpy.zeros((240, 320, 3), dtype=numpy.uint8)
self.inputImage = yarp.ImageRgb()
self.tempImage = yarp.ImageRgb()
self.tempImage.resize(320, 240)
self.tempImage.setExternal(self.imgArray, self.imgArray.shape[1],
self.imgArray.shape[0])
self.outputImage = yarp.ImageRgb()
self.outputStill = yarp.ImageRgb()
self.uniCom = 'None'
print 'configured'
return True
def get_image(self):
# Create image placeholder
img = np.zeros((self.height, self.width, 3), dtype=np.uint8)
# Read image with yarp
yarp_img = yarp.ImageRgb()
yarp_img.resize(self.width, self.height)
yarp_img.setExternal(img, self.width, self.height)
if not self.port.read(yarp_img):
raise ValueError("Try to read a closed port")
# Check image
if not yarp_img.getRawImage().__long__() == img.__array_interface__['data'][0]:
raise RuntimeError("read() reallocated my yarp_img")
return img
def __init__(self, app_name, port_name):
# open recipient port
self.port = yarp.Port()
self.port.open(app_name + port_name)
yarp.delay(0.25)
# connect the port to camera
yarp.Network.connect(port_name, app_name + port_name)
yarp.delay(0.25)
# prepare data buffer for reception
self.width = 320
self.height = 240
self.yarp_img = yarp.ImageRgb()
self.yarp_img.resize(self.width, self.height)
self.array_img = bytearray(self.width * self.height * 3)
self.yarp_img.setExternal(self.array_img, self.width, self.height)
# prepare blank image to be returned when an error appears
self.blank = np.zeros(self.shape())
def yarp_to_python(self):
# Create a port and connect it to the iCub simulator virtual camera
# Create numpy array to receive the image and the YARP image wrapped around it
img_array = numpy.zeros((240, 320, 3), dtype=numpy.uint8)
yarp_image = yarp.ImageRgb()
yarp_image.resize(320, 240)
yarp_image.setExternal(img_array, img_array.shape[1], img_array.shape[0])
# Alternatively, if using Python 2.x, try:
#yarp_image.setExternal(img_array.__array_interface__['data'][0], img_array.shape[1], img_array.shape[0])
# Read the data from the port into the image
self.input_port.read(yarp_image)
# display the image that has been read
# matplotlib.pylab.imshow(img_array)
cv2.imshow('frame', self.detect_colour(img_array))
def yarp_to_python(input_port):
# Create a port and connect it to the iCub simulator virtual camera
# input_port = yarp.BufferedPortImageRgb()
# input_port.open("/python-image-port")
# yarp.Network.connect("/icub/camcalib/left/out", "/python-image-port")
# Create numpy array to receive the image and the YARP image wrapped around it
img_array = numpy.ones((240, 320, 3), dtype=numpy.uint8)
yarp_image = yarp.ImageRgb()
yarp_image.resize(320, 240)
yarp_image.setExternal(img_array, img_array.shape[1], img_array.shape[0])
# Alternatively, if using Python 2.x, try:
# yarp_image.setExternal(img_array.__array_interface__['data'][0], img_array.shape[1], img_array.shape[0])
# Read the data from the port into the image
input_port.read(yarp_image)
return img_array, yarp_image
def __init__(self, icub_root='/icubSim'):
# Global variables
self.thread_movement_detection = threading.Thread(target=None)
self.acapela_account_login = ''
self.acapela_application_login = ''
self.acapela_application_password = ''
self.acapela_service_url = ''
# Deepgaze variables
self.object_list = list()
self.histogram_classifier = HistogramColorClassifier(
channels=[0, 1, 2],
hist_size=[128, 128, 128],
hist_range=[0, 256, 0, 256, 0, 256],
hist_type='BGR')
# Init YARP
yarp.Network.init()
# Camera connection
try:
cam_w = 320 # 640
cam_h = 240 # 480
# Left camera
print("[ICUB] Init: Waiting for " + icub_root + "/cam/left' ...")
self.port_left_camera = yarp.Port()
self.port_left_camera.open("/pyera-left-image-port")
yarp.Network.connect(icub_root + "/cam/left",
"/pyera-left-image-port")
# right camera
print("[ICUB] Init: Waiting for " + icub_root + "/cam/right' ...")
self.port_right_camera = yarp.Port()
self.port_right_camera.open("/pyera-right-image-port")
yarp.Network.connect(icub_root + "/cam/right",
"/pyera-right-image-port")
# Set the numpy array to fill with the image
self.img_array = np.zeros((cam_h, cam_w, 3), dtype=np.uint8)
self.yarp_image = yarp.ImageRgb()
self.yarp_image.resize(cam_w, cam_h)
self.yarp_image.setExternal(self.img_array,
self.img_array.shape[1],
self.img_array.shape[0])
except BaseException, err:
print("[ICUB][ERROR] connect To Camera catching error " + str(err))
return
def writeImageYarpPort(self, imgArray, yarpPort):
""" Send the image through the yarp port
param img_array: image data in the form of numpy array [height, width, 3].
Image to be sent.
"""
yarpImg = yarpPort.prepare(
) # Get a place to store the image to be sent
img = yarp.ImageRgb()
img.resize(imgArray.shape[1], imgArray.shape[0])
img.setExternal(
imgArray.data, imgArray.shape[1],
imgArray.shape[0]) # Wrap yarp image around the numpy array
yarpImg.copy(img)
yarpPort.write()
def write_image_yarp_port(self, img_array):
""" Send the image through the yarp port
param img_array: image data in the form of numpy array [height, width, 3].
Image to be sent.
"""
yarp_img = self.glasses_images_port.prepare(
) # Get a place to store the image to be sent
img = yarp.ImageRgb()
img.resize(self.imageWidth, self.imageHeight)
img.setExternal(
img_array.data, self.imageWidth,
self.imageHeight) # Wrap yarp image around the numpy array
yarp_img.copy(img)
print(yarp_img)
self.glasses_images_port.write()
def yarp_get():
img_array = numpy.ones((480, 640, 3), dtype=numpy.uint8)
source = img_array
bitmap = cv.CreateImageHeader((source.shape[1], source.shape[0]),
cv.IPL_DEPTH_8U, 3)
cv.SetData(bitmap, source.tostring(),
source.dtype.itemsize * 3 * source.shape[1])
img_array = bitmap
yarp_image = yarp.ImageRgb()
yarp_image.resize(640, 480)
yarp_image.setExternal(img_array, img_array.shape[1], img_array.shape[0])
# print img_array.__array_interface__['data'][0]
#yarp_image.setExternal(img_array.__array_interface__['data'][0], img_array.shape[1], img_array.shape[0])
input_port.read(yarp_image)
# print img_array.getIplImage()
plt.imshow(img_array)
#plt.show()
return img_array
def processLiveData(self, dataList, thisModel):
print 'process live data'
print len(dataList)
imgH = thisModel[0].paramsDict['imgH']
imgW = thisModel[0].paramsDict['imgW']
imgHNew = thisModel[0].paramsDict['imgHNew']
imgWNew = thisModel[0].paramsDict['imgWNew']
numFaces = len(dataList)
imageArray = numpy.zeros((imgH, imgW, 3), dtype=numpy.uint8)
yarpImage = yarp.ImageRgb()
yarpImage.resize(imgH, imgW)
yarpImage.setExternal(imageArray, imageArray.shape[1],
imageArray.shape[0])
# images = numpy.zeros((numFaces, imgHNew * imgWNew), dtype=numpy.uint8)
labels = [None] * numFaces
likelihoods = [None] * numFaces
if numFaces > 0:
# average all faces
for i in range(numFaces):
yarpImage.copy(dataList[i])
imageArrayOld = cv2.resize(imageArray, (imgHNew, imgWNew))
imageArrayGray = cv2.cvtColor(imageArrayOld,
cv2.COLOR_BGR2GRAY)
instance = imageArrayGray.flatten()[None, :]
print instance.shape
print "Collected face: " + str(i)
[labels[i], likelihoods[i]
] = SAMTesting.testSegment(thisModel, instance, True, None)
return SAMTesting.combineClassifications(thisModel, labels,
likelihoods)
else:
return [None, 0]
def yarp_to_python():
# Create a port and connect it to the iCub simulator virtual camera
input_port = yarp.Port()
input_port.open("/python-image-port")
yarp.Network.connect("/icubSim/cam", "/python-image-port")
# Create numpy array to receive the image and the YARP image wrapped around it
img_array = numpy.zeros((240, 320, 3), dtype=numpy.uint8)
yarp_image = yarp.ImageRgb()
yarp_image.resize(320, 240)
yarp_image.setExternal(img_array, img_array.shape[1], img_array.shape[0])
# Alternatively, if using Python 2.x, try:
# yarp_image.setExternal(img_array.__array_interface__['data'][0], img_array.shape[1], img_array.shape[0])
# Read the data from the port into the image
input_port.read(yarp_image)
# display the image that has been read
matplotlib.pylab.imshow(img_array)
# Cleanup
input_port.close()
# Port for left eye image
input_port_left_eye = yarp.Port()
if not input_port_left_eye.open("/" + CLIENT_PREFIX + "/eyes/left"):
print("[ERROR] Could not open left eye port")
if not yarp.Network.connect("/" + ROBOT_PREFIX + "/cam/left",
"/" + CLIENT_PREFIX + "/eyes/left"):
print("[ERROR] Could not connect input_port_left_eye")
######################################################################
################## Initialization of both eye images #################
print('----- Init image array structures -----')
# Create numpy array to receive the image and the YARP image wrapped around it
left_eye_img_array = np.ones((240, 320, 3), np.uint8)
left_eye_yarp_image = yarp.ImageRgb()
left_eye_yarp_image.resize(320, 240)
right_eye_img_array = np.ones((240, 320, 3), np.uint8)
right_eye_yarp_image = yarp.ImageRgb()
right_eye_yarp_image.resize(320, 240)
left_eye_yarp_image.setExternal(left_eye_img_array.data,
left_eye_img_array.shape[1],
left_eye_img_array.shape[0])
right_eye_yarp_image.setExternal(right_eye_img_array.data,
right_eye_img_array.shape[1],
right_eye_img_array.shape[0])
#objects_iCubSim()
######################################################################
def configure(self, rf):
'''
Configure the module internal variables and ports according to resource finder
'''
self._rf = rf
# Input
# Image port initialization
self._port_in = yarp.BufferedPortImageRgb()
self._port_in.open('/' + self._module_name + '/RGBimage:i')
# Input buffer initialization
self._input_buf_image = yarp.ImageRgb()
self._input_buf_image.resize(self._input_img_width,
self._input_img_height)
self._input_buf_array = np.ones(
(self._input_img_height, self._input_img_width, 3), dtype=np.uint8)
self._input_buf_image.setExternal(self._input_buf_array,
self._input_buf_array.shape[1],
self._input_buf_array.shape[0])
print('Input image buffer configured')
# Output
# Output image port initialization
self._port_out = yarp.Port()
self._port_out.open('/' + self._module_name + '/RGBimage:o')
# Output blobs port initialization
self._port_out_bboxes = yarp.Port()
self._port_out_bboxes.open('/' + self._module_name + '/bboxes:o')
# Output detection info port initialization
self._port_out_info = yarp.Port()
self._port_out_info.open('/' + self._module_name + '/detectionInfo:o')
# Output buffer initialization
self._output_buf_image = yarp.ImageRgb()
self._output_buf_image.resize(self._input_img_width,
self._input_img_height)
self._output_buf_array = np.zeros(
(self._input_img_height, self._input_img_width, 3), dtype=np.uint8)
self._output_buf_image.setExternal(self._output_buf_array,
self._output_buf_array.shape[1],
self._output_buf_array.shape[0])
print('Output image buffer configured')
# Output mask port initialization
self._port_out_mask = yarp.Port()
self._port_out_mask.open('/' + self._module_name + '/maskImage:o')
# Output mask buffer initialization
self._output_mask_buf_image = yarp.ImageMono()
self._output_mask_buf_image.resize(self._input_img_width,
self._input_img_height)
print('Output mask buffer configured')
# RPC port initialization
self._port_rpc = yarp.RpcServer()
self._port_rpc.open('/' + self._module_name + '/rpc')
self.attach_rpc_server(self._port_rpc)
# Inference model setup
# Configure some parameters for inference
config = configurations.YCBVideoConfigInference()
config.POST_NMS_ROIS_INFERENCE = 300
config.PRE_NMS_LIMIT = 1000
config.DETECTION_MAX_INSTANCES = 10
config.DETECTION_MIN_CONFIDENCE = 0.75
config.display()
self._model = modellib.MaskRCNN(mode='inference',
model_dir=MODEL_DIR,
config=config)
self._detection_results = None
print('Inference model configured')
# Load class names
dataset_root = os.path.join(ROOT_DIR, "datasets",
"bottles_ycb_video_format")
# Automatically discriminate the dataset according to the config file
if isinstance(config, configurations.TabletopConfigInference):
# Load the validation dataset
self._dataset = datasets.TabletopDataset()
elif isinstance(config, configurations.YCBVideoConfigInference):
self._dataset = datasets.YCBVideoDataset()
# No need to load the whole dataset, just the class names will be ok
self._dataset.load_class_names(dataset_root)
# Create a dict for assigning colors to each class
random_class_colors = tabletop_bottles.random_colors(
len(self._dataset.class_names))
self._class_colors = {
class_id: color
for (color, class_id
) in zip(random_class_colors, self._dataset.class_names)
}
# Load model weights
try:
assert os.path.exists(self._model_weights_path)
except AssertionError as error:
print("Model weights path invalid: file does not exist")
print(error)
return False
self._model.load_weights(self._model_weights_path, by_name=True)
print("Model weights loaded")
return True
