def step(self): stamp = super().step() if not stamp: return # Publish camera data if self._image_publisher.get_subscription_count( ) > 0 or self._always_publish: self._wb_device.enable(self._timestep) # Image data msg = Image() msg.header.stamp = stamp msg.header.frame_id = self._frame_id msg.height = self._wb_device.getHeight() msg.width = self._wb_device.getWidth() msg.is_bigendian = False msg.step = self._wb_device.getWidth() * 4 if self._wb_device.getRangeImage() is None: return image_array = np.array(self._wb_device.getRangeImage(), dtype="float32") # We pass `data` directly to we avoid using `data` setter. # Otherwise ROS2 converts data to `array.array` which slows down the simulation as it copies memory internally. # Both, `bytearray` and `array.array`, implement Python buffer protocol, so we should not see unpredictable # behavior. # deepcode ignore W0212: Avoid conversion from `bytearray` to `array.array`. msg._data = image_array.tobytes() msg.encoding = '32FC1' self._image_publisher.publish(msg) else: self._wb_device.disable()
def step(self): stamp = super().step() if not stamp: return # Publish camera data if self._image_publisher.get_subscription_count( ) > 0 or self._always_publish: self._wb_device.enable(self._timestep) image = self._wb_device.getImage() if image is None: return # Image data msg = Image() msg.header.stamp = stamp msg.header.frame_id = self._frame_id msg.height = self._wb_device.getHeight() msg.width = self._wb_device.getWidth() msg.is_bigendian = False msg.step = self._wb_device.getWidth() * 4 # We pass `data` directly to we avoid using `data` setter. # Otherwise ROS2 converts data to `array.array` which slows down the simulation as it copies memory internally. # Both, `bytearray` and `array.array`, implement Python buffer protocol, so we should not see unpredictable # behavior. # deepcode ignore W0212: Avoid conversion from `bytearray` to `array.array`. msg._data = image msg.encoding = 'bgra8' self._image_publisher.publish(msg) self.__message_info.header.stamp = Time( seconds=self._node.robot.getTime()).to_msg() self._camera_info_publisher.publish(self.__message_info) if self._wb_device.hasRecognition( ) and self._recognition_publisher.get_subscription_count() > 0: self._wb_device.recognitionEnable(self._timestep) objects = self._wb_device.getRecognitionObjects() if objects is None: return # Recognition data msg = RecognitionObjects() msg.header.stamp = stamp msg.header.frame_id = self._frame_id for obj in objects: msg_obj = RecognitionObject() msg_obj.position = obj.get_position() msg_obj.position_on_image = obj.get_position_on_image() msg_obj.size_on_image = obj.get_size_on_image() msg_obj.number_of_colors = obj.get_number_of_colors() msg_obj.colors = obj.get_colors() msg_obj.model = str(obj.get_model()) msg.objects.append(msg_obj) self._recognition_publisher.publish(msg) else: self._wb_device.recognitionDisable() else: self._wb_device.disable()
def imageCallback(self, msg): '''! Recebe a imagem, seg,emta e publica ''' img = np.array(msg.data, dtype=np.uint8) img = np.array(np.split(img, msg.height)) img = np.array(np.split(img, msg.width, axis=1)) img = np.rot90(img) img = tf.keras.preprocessing.image.array_to_img(img) image_array = tf.keras.preprocessing.image.img_to_array(img) result = self.bodypix_model.predict_single(image_array) mask = result.get_mask(threshold=0.75) colored_mask = result.get_colored_part_mask(mask) mask = mask.numpy().astype(np.uint8) colored_mask = colored_mask.astype(np.uint8) maskMsg = Image() maskMsg._data = mask.flatten().tolist() maskMsg.height = mask.shape[0] maskMsg.width = mask.shape[1] maskMsg.encoding = "8UC1" maskMsg.is_bigendian = 0 maskMsg.step = maskMsg.width colMsg = Image() colMsg._data = colored_mask.flatten().tolist() colMsg.height = colored_mask.shape[0] colMsg.width = colored_mask.shape[1] colMsg.encoding = "8UC3" colMsg.is_bigendian = 0 colMsg.step = colMsg.width * 3 maskMsg.header = msg.header colMsg.header = msg.header self.pubMask.publish(maskMsg) self.colMask.publish(colMsg)
def timerCallback(self): '''! Callback do timer que recebe a publica a imagem ''' frameId = self.get_parameter( "frame_id").get_parameter_value().string_value tempo1 = cv.getTickCount() if (self.ret): self.ret = False imgMsg = Image() imgMsg._data = self.flattened imgMsg.height = self.height imgMsg.width = self.width imgMsg.encoding = "8UC3" imgMsg.is_bigendian = 0 imgMsg.step = imgMsg.width * 3 time = self.get_clock().now().to_msg() imgMsg.header.stamp = time imgMsg.header.frame_id = frameId self.pubImg.publish(imgMsg) #compImgMsg = bridge.cv2_to_compressed_imgmsg(frame) #compImgMsg.header.stamp = time #compImgMsg.header.frame_id = frameId #self.pubImgCompress.publish(compImgMsg) self.publishInfo(imgMsg.header) tempo2 = cv.getTickCount()
def step(self): stamp = super().step() if not stamp: return # Publish camera data if self._image_publisher.get_subscription_count( ) > 0 or self._always_publish: self._wb_device.enable(self._timestep) image = self._wb_device.getImage() if image is None: return # Image data msg = Image() msg.header.stamp = stamp msg.header.frame_id = self._frame_id msg.height = self._wb_device.getHeight() msg.width = self._wb_device.getWidth() msg.is_bigendian = False msg.step = self._wb_device.getWidth() * 4 # We pass `data` directly to we avoid using `data` setter. # Otherwise ROS2 converts data to `array.array` which slows down the simulation as it copies memory internally. # Both, `bytearray` and `array.array`, implement Python buffer protocol, so we should not see unpredictable # behavior. # deepcode ignore W0212: Avoid conversion from `bytearray` to `array.array`. msg._data = image msg.encoding = 'bgra8' self._image_publisher.publish(msg) self.__message_info.header.stamp = Time( seconds=self._node.robot.getTime()).to_msg() self._camera_info_publisher.publish(self.__message_info) if self._wb_device.hasRecognition( ) and (self._recognition_publisher.get_subscription_count() > 0 or self._recognition_webots_publisher.get_subscription_count() > 0): self._wb_device.recognitionEnable(self._timestep) objects = self._wb_device.getRecognitionObjects() if objects is None: return # Recognition data reco_msg = Detection2DArray() reco_msg_webots = WbCameraRecognitionObjects() reco_msg.header.stamp = stamp reco_msg_webots.header.stamp = stamp reco_msg.header.frame_id = self._frame_id reco_msg_webots.header.frame_id = self._frame_id for obj in objects: # Getting Object Info position = Point() orientation = Quaternion() position.x = obj.get_position()[0] position.y = obj.get_position()[1] position.z = obj.get_position()[2] axangle = obj.get_orientation() quat = axangle2quat(axangle[:-1], axangle[-1]) orientation.w = quat[0] orientation.x = quat[1] orientation.y = quat[2] orientation.z = quat[3] obj_model = obj.get_model().decode('UTF-8') obj_center = [ float(i) for i in obj.get_position_on_image() ] obj_size = [float(i) for i in obj.get_size_on_image()] obj_id = obj.get_id() obj_colors = obj.get_colors() # Object Info -> Detection2D reco_obj = Detection2D() hyp = ObjectHypothesisWithPose() hyp.id = obj_model hyp.pose.pose.position = position hyp.pose.pose.orientation = orientation reco_obj.results.append(hyp) reco_obj.bbox.center.x = obj_center[0] reco_obj.bbox.center.y = obj_center[1] reco_obj.bbox.size_x = obj_size[0] reco_obj.bbox.size_y = obj_size[1] reco_msg.detections.append(reco_obj) # Object Info -> WbCameraRecognitionObject reco_webots_obj = WbCameraRecognitionObject() reco_webots_obj.id = obj_id reco_webots_obj.model = obj_model reco_webots_obj.pose.pose.position = position reco_webots_obj.pose.pose.orientation = orientation reco_webots_obj.bbox.center.x = obj_center[0] reco_webots_obj.bbox.center.y = obj_center[1] reco_webots_obj.bbox.size_x = obj_size[0] reco_webots_obj.bbox.size_y = obj_size[1] for i in range(0, obj.get_number_of_colors()): color = ColorRGBA() color.r = obj_colors[3 * i] color.g = obj_colors[3 * i + 1] color.b = obj_colors[3 * i + 2] reco_webots_obj.colors.append(color) reco_msg_webots.objects.append(reco_webots_obj) self._recognition_webots_publisher.publish(reco_msg_webots) self._recognition_publisher.publish(reco_msg) else: self._wb_device.recognitionDisable() else: self._wb_device.disable()