def airpub(): pub_left = rospy.Publisher("/gi/simulation/left/image_raw", Image, queue_size=1) pub_right=rospy.Publisher("/gi/simulation/right/image_raw", Image, queue_size=1) rospy.init_node('airsim_info', anonymous=True) rate = rospy.Rate(10) # 10hz # connect to the AirSim simulator client = airsim.MultirotorClient() client.confirmConnection() while not rospy.is_shutdown(): # get camera images from the car Imu = client.getImuData() responses = client.simGetImages([ airsim.ImageRequest("front-left", airsim.ImageType.Scene, False, False), airsim.ImageRequest("front-right", airsim.ImageType.Scene, False, False) ]) img_dic={} for idx, response in enumerate(responses): img_rgb_string = response.image_data_uint8 img_dic[idx]=img_rgb_string # Populate image message msg_left=Image() msg_left.header.stamp = rospy.Time.now() msg_left.header.frame_id = "frameId" msg_left.encoding = "rgb8" msg_left.height = 480 # resolution should match values in settings.json msg_left.width = 752 msg_left.data = img_dic[0] msg_left.is_bigendian = 0 msg_left.step = msg_left.width * 3 msg_right = Image() msg_right.header.stamp = rospy.Time.now() msg_right.header.frame_id = "frameId" msg_right.encoding = "rgb8" msg_right.height = 480 # resolution should match values in settings.json msg_right.width = 752 msg_right.data = img_dic[1] msg_right.is_bigendian = 0 msg_right.step = msg_left.width * 3 # log time and size of published image rospy.loginfo(len(response.image_data_uint8)) # publish image message pub_left.publish(msg_left) pub_right.publish(msg_right) # sleep until next cycle rate.sleep()
def main(): # img_height = 1208 # img_width = 1920 * 2 # _rate = 1 rospy.init_node("generate_fake_img", anonymous=True) img_height = int(rospy.get_param('img_height', 1208)) img_width = int(rospy.get_param('img_width', 1920)) * 2 _rate = int(rospy.get_param('fps', 30)) pub = rospy.Publisher('simulate_camera_output_pub', Image, queue_size=10) rate = rospy.Rate(_rate) while not rospy.is_shutdown(): global count count = count + 1 # img = generate_fake_img_random() img = generate_pattern(img_height, img_width) msg = Image() msg.data = img.tostring() msg.header.stamp = rospy.Time.now() msg.height = img_height msg.width = img_width / 2 msg.encoding = 'bayer_rggb16' msg.is_bigendian = 0 msg.step = img_width pub.publish(msg) rate.sleep()
def _build_result_msg(self, msg, result): result_msg = Result() result_msg.header = msg.header for i, (y1, x1, y2, x2) in enumerate(result['rois']): box = RegionOfInterest() box.x_offset = x1#np.asscalar(x1) box.y_offset = y1#np.asscalar(y1) box.height = y2-y1#np.asscalar(y2 - y1) box.width = x2-x1#np.asscalar(x2 - x1) result_msg.boxes.append(box) class_id = result['class_ids'][i] result_msg.class_ids.append(class_id) class_name = self._class_names[class_id] result_msg.class_names.append(class_name) score = result['scores'][i] result_msg.scores.append(score) mask = Image() mask.header = msg.header mask.height = result['masks'].shape[0] mask.width = result['masks'].shape[1] mask.encoding = "mono8" mask.is_bigendian = False mask.step = mask.width mask.data = (result['masks'][:, :, i] * 255).tobytes() result_msg.masks.append(mask) return result_msg
def _get_images(self): # get camera images from the car responses = self.client.simGetImages([ airsim.ImageRequest("1", airsim.ImageType.Scene, False, False), airsim.ImageRequest("2", airsim.ImageType.Scene, False, False)]) #scene vision image in uncompressed RGBA array msgs = [] stamp = rospy.Time.now() for response in responses: img_rgba_string = response.image_data_uint8 # Populate image message msg = Image() msg.header.stamp = stamp msg.header.frame_id = "frameId" msg.encoding = "rgba8" msg.height = response.height msg.width = response.width msg.data = img_rgba_string msg.is_bigendian = 0 msg.step = msg.width * 4 array = self.bridge.imgmsg_to_cv2(msg, "rgba8") grayscale = cv2.cvtColor(array, cv2.COLOR_RGBA2GRAY) converted = self.bridge.cv2_to_imgmsg(grayscale, "8UC1") converted.header.stamp = stamp msgs.append(converted) assert(len(msgs) == 2) rospy.loginfo(len(response.image_data_uint8)) return msgs
def airpub(): pub = rospy.Publisher("airsim/image_raw", Image, queue_size=1) rospy.init_node('image_raw', anonymous=True) rate = rospy.Rate(10) # 10hz # connect to the AirSim simulator client = airsim.MultirotorClient() client.confirmConnection() while not rospy.is_shutdown(): # get camera images from the car responses = client.simGetImages([ airsim.ImageRequest("1", airsim.ImageType.Scene, False, False) ]) #scene vision image in uncompressed RGB array for response in responses: img_rgb_string = response.image_data_uint8 # Populate image message msg = Image() msg.header.stamp = rospy.Time.now() msg.header.frame_id = "frameId" msg.encoding = "rgb8" msg.height = 360 # resolution should match values in settings.json msg.width = 640 msg.data = img_rgb_string msg.is_bigendian = 0 msg.step = msg.width * 3 # log time and size of published image rospy.loginfo(len(response.image_data_uint8)) # publish image message pub.publish(msg) # sleep until next cycle rate.sleep()
def publish_processed_image(self): # draw box and caption string self.rgb = cv2.rectangle(self.rgb, (self.bbox[0], self.bbox[1]), (self.bbox[2],self.bbox[3]), (0,0,0xFF), 2) self.rgb = cv2.putText(self.rgb, self.caption, (self.bbox[0]+2, self.bbox[1]-5), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0,0,0xFF), thickness=2) #construct sensor_msgs/Image object self.imgmsg.data = self.rgb.tobytes() # publish frame rospy.loginfo('publishing processed frame (with bbox) %s' % rospy.get_time()) self.rgb1_pub.publish(self.imgmsg) # hack to republish frame msk = np.zeros( (self.rgb.shape[0], self.rgb.shape[1])) msk = cv2.rectangle(msk, (self.bbox[0], self.bbox[1]), (self.bbox[2],self.bbox[3]), 0xFF, -1) rgb_msked = np.zeros_like(self.rgb) rgb_msked[msk==0xFF, :] = self.rgb[msk==0xFF, :] # construct new imgmsg msg = Image() msg.header = self.imgmsg.header msg.height, msg.width, msg.step = self.imgmsg.height, self.imgmsg.width, self.imgmsg.step msg.encoding = self.imgmsg.encoding msg.is_bigendian = self.imgmsg.is_bigendian msg.data = rgb_msked.tobytes() self.rgb2_pub.publish(msg) self.rate.sleep() # release GPU memory for next data del self.img
def appendMessages(self, stamp, messages): if not self._image is None: # Build the image message and push it on the message list msg = Image() msg.header.stamp = stamp msg.header.frame_id = self._frameId msg.width = self._image.shape[0] msg.height = self._image.shape[1] if (len(self._image.shape) == 2) or (len(self._image.shape) == 3 and self._image.shape[2] == 1): # A gray image msg.encoding = '8UC1' stepMult = 1 elif len(self._image.shape) == 3 and self._image.shape[2] == 3: # A color image msg.encoding = 'rgb8' stepMult = 3 elif len(self._image.shape) == 3 and self._image.shape[2] == 4: # A color image msg.encoding = 'rgba8' stepMult = 3 else: raise RuntimeError("The parsing of images is very simple. " +\ "Only 3-channel rgb (rgb8), 4 channel rgba " +\ "(rgba8) and 1 channel mono (mono8) are " +\ "supported. Got an image with shape " +\ "{0}".format(self._image.shape)) msg.is_bigendian = False msg.step = stepMult * msg.width msg.data = self._image.flatten().tolist() messages.append((self._topic, msg))
def convert(data): width=data.info.width height=data.info.height pixelList = [0] *width*height imageMsg=Image() imageMsg.header.stamp = rospy.Time.now() imageMsg.header.frame_id = '1' imageMsg.height = height imageMsg.width = width imageMsg.encoding = 'mono8' imageMsg.is_bigendian = 0 imageMsg.step = width for h in range(height): for w in range(width): if data.data[h*width+w]==-1: pixelList[h*width+w] = 150 elif data.data[h*width+w]==0: pixelList[h*width+w] = 0 elif data.data[h*width+w]==100: pixelList[h*width+w] = 255 else: pixelList[h*width+w]=data.data[h*width+w] print 'ERROR' imageMsg.data = pixelList imagePub.publish(imageMsg)
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 record_rgb(): """Recording 2D visuals """ # Get the RGB image from the Kinect sensor # Note: There are issues with wait_for_message not getting the # most up to date message from /camera/rgb/image_raw. # This is a hack to wait for the updated message current_time = rospy.get_rostime() orig_rgb = rospy.wait_for_message('/camera/rgb/image_raw', Image) while orig_rgb.header.stamp < current_time: orig_rgb = rospy.wait_for_message('/camera/rgb/image_raw', Image) # Copy the recevied Image message to a new Image message copy_rgb = Image() copy_rgb.header = orig_rgb.header copy_rgb.height = orig_rgb.height copy_rgb.width = orig_rgb.width copy_rgb.encoding = orig_rgb.encoding copy_rgb.is_bigendian = orig_rgb.is_bigendian copy_rgb.step = orig_rgb.step copy_rgb.data = orig_rgb.data # Publish the new Image to be saved with the specific name rgb_pub = rospy.Publisher('rgb_image', Image, queue_size=1) rospy.loginfo('-------------------------------------------------') rospy.loginfo('Generating RGB') rospy.loginfo('-------------------------------------------------') rospy.sleep(3) rgb_pub.publish(copy_rgb)
def timer_callback(self): """Timer Callback Function This method captures images and publishes required data in ros 2 topic. """ if self.capture.isOpened(): # reads image data ret, frame = self.capture.read() # processes image data and converts to ros 2 message msg = Image() msg.header.stamp = Node.get_clock(self).now().to_msg() msg.header.frame_id = 'ANI717' msg.height = np.shape(frame)[0] msg.width = np.shape(frame)[1] msg.encoding = "bgr8" msg.is_bigendian = False msg.step = np.shape(frame)[2] * np.shape(frame)[1] msg.data = np.array(frame).tobytes() # publishes message self.publisher_.publish(msg) self.get_logger().info('%d Images Published' % self.i) # image counter increment self.i += 1 return None
def applyDepthMask(self, image_msg, mask_msg, blur): # height and width height = image_msg.height width = image_msg.width # get images image_raw = np.fromstring(image_msg.data, np.uint8) image_blur = np.full((height * width * 3, 1), 225) mask = np.fromstring(mask_msg.data, np.uint8) # reshape arrrays (fromstring) to matrices mask = mask.reshape(height * width, 1) image_raw = image_raw.reshape(height * width * 3, 1) # for index i in images, # select image_raw[i] when mask[i] true # select image_blur[i] when mask[i] false image_masked = np.empty(image_raw.shape) for i in range(3): image_masked[i::3] = np.where(mask, image_raw[i::3], image_blur[i::3]) # create sensor_msgs image image_msg_out = Image() image_msg_out.header.stamp = image_msg.header.stamp image_msg_out.header.frame_id = image_msg.header.frame_id image_msg_out.height = image_msg.height image_msg_out.width = image_msg.width image_msg_out.encoding = image_msg.encoding image_msg_out.is_bigendian = image_msg.is_bigendian image_msg_out.step = image_msg.step image_msg_out.data = list(image_masked) # result return image_msg_out
def publishImage(self, type="color"): responses = self.client.simGetImages([ airsim.ImageRequest("1", airsim.ImageType.Scene, False, False) ]) #scene vision image in uncompressed RGB array for response in responses: img_rgb_string = response.image_data_uint8 # Populate image message msg = Image() msg.header.stamp = rospy.Time.now() msg.header.frame_id = "frameId" msg.encoding = "rgb8" msg.height = 360 # resolution should match values in settings.json msg.width = 640 msg.data = img_rgb_string msg.is_bigendian = 0 msg.step = msg.width * 4 # log time and size of published image rospy.loginfo(len(response.image_data_uint8)) # publish image message # msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring() self.image_pub.publish(msg) # sleep until next cycle rospy.Rate(10).sleep()
def camera_callback(self): # Image data msg = Image() msg.height = self.camera.getHeight() msg.width = self.camera.getWidth() msg.is_bigendian = False msg.step = self.camera.getWidth() * 4 msg.data = self.camera.getImage() msg.encoding = 'bgra8' self.camera_publisher.publish(msg) # CameraInfo data msg = CameraInfo() msg.header.frame_id = 'camera_frame' msg.height = self.camera.getHeight() msg.width = self.camera.getWidth() msg.distortion_model = 'plumb_bob' msg.d = [0.0, 0.0, 0.0, 0.0, 0.0] msg.k = [ self.camera.getFocalLength(), 0.0, self.camera.getWidth() / 2, 0.0, self.camera.getFocalLength(), self.camera.getHeight() / 2, 0.0, 0.0, 1.0 ] msg.p = [ self.camera.getFocalLength(), 0.0, self.camera.getWidth() / 2, 0.0, 0.0, self.camera.getFocalLength(), self.camera.getHeight() / 2, 0.0, 0.0, 0.0, 1.0, 0.0 ] self.camera_info_publisher.publish(msg)
def cv2_to_imgmsg(cvim, encoding="passthrough"): """ Convert an OpenCV :cpp:type:`cv::Mat` type to a ROS sensor_msgs::Image message. :param cvim: An OpenCV :cpp:type:`cv::Mat` :param encoding: The encoding of the image data, one of the following strings: * ``"passthrough"`` * one of the standard strings in sensor_msgs/image_encodings.h :rtype: A sensor_msgs.msg.Image message :raises CvBridgeError: when the ``cvim`` has a type that is incompatible with ``encoding`` If encoding is ``"passthrough"``, then the message has the same encoding as the image's OpenCV type. Otherwise desired_encoding must be one of the standard image encodings This function returns a sensor_msgs::Image message on success, or raises :exc:`cv_bridge.CvBridgeError` on failure. """ import numpy as np if not isinstance(cvim, (np.ndarray, np.generic)): raise TypeError('Your input type is not a numpy array') img_msg = Image() img_msg.height = cvim.shape[0] img_msg.width = cvim.shape[1] cv_type = '8UC3' if encoding == "passthrough": img_msg.encoding = cv_type else: img_msg.encoding = encoding if cvim.dtype.byteorder == '>': img_msg.is_bigendian = True img_msg.data = cvim.tostring() img_msg.step = len(img_msg.data) // img_msg.height return img_msg
def publish( self ): # Get the image. image = self.__videoDeviceProxy.getImageRemote( self.__videoDeviceProxyName ); # Create Image message. ImageMessage = Image(); ImageMessage.header.stamp.secs = image[ 5 ]; ImageMessage.width = image[ 0 ]; ImageMessage.height = image[ 1 ]; ImageMessage.step = image[ 2 ] * image[ 0 ]; ImageMessage.is_bigendian = False; ImageMessage.encoding = 'bgr8'; ImageMessage.data = image[ 6 ]; self.__imagePublisher.publish( ImageMessage ); # Create CameraInfo message. # Data from the calibration phase is hard coded for now. CameraInfoMessage = CameraInfo(); CameraInfoMessage.header.stamp.secs = image[ 5 ]; CameraInfoMessage.width = image[ 0 ]; CameraInfoMessage.height = image[ 1 ]; CameraInfoMessage.D = [ -0.0769218451517258, 0.16183180613612602, 0.0011626049774280595, 0.0018733894100460534, 0.0 ]; CameraInfoMessage.K = [ 581.090096189648, 0.0, 341.0926325830606, 0.0, 583.0323248080421, 241.02441593704128, 0.0, 0.0, 1.0 ]; CameraInfoMessage.R = [ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 ]; CameraInfoMessage.P = [ 580.5918359588198, 0.0, 340.76398441334106, 0.0, 0.0, 582.4042541534321, 241.04182225157172, 0.0, 0.0, 0.0, 1.0, 0.0] ; CameraInfoMessage.distortion_model = 'plumb_bob'; #CameraInfoMessage.roi.x_offset = self.__ROIXOffset; #CameraInfoMessage.roi.y_offset = self.__ROIYOffset; #CameraInfoMessage.roi.width = self.__ROIWidth; #CameraInfoMessage.roi.height = self.__ROIHeight; #CameraInfoMessage.roi.do_rectify = self.__ROIDoRectify; self.__cameraInfoPublisher.publish( CameraInfoMessage );
def airpub(): pub = rospy.Publisher("airsim/image_raw", Image, queue_size=1) rospy.init_node('image_raw', anonymous=True) rate = rospy.Rate(10) # 10hz # connect to the AirSim simulator client = airsim.MultirotorClient() client.confirmConnection() while not rospy.is_shutdown(): # get camera images from the car responses = client.simGetImages([ airsim.ImageRequest("1", airsim.ImageType.Scene, False, False)]) #scene vision image in uncompressed RGBA array for response in responses: img_rgba_string = response.image_data_uint8 # Populate image message msg=Image() msg.header.stamp = rospy.Time.now() msg.header.frame_id = "frameId" msg.encoding = "rgba8" msg.height = 360 # resolution should match values in settings.json msg.width = 640 msg.data = img_rgba_string msg.is_bigendian = 0 msg.step = msg.width * 4 # log time and size of published image rospy.loginfo(len(response.image_data_uint8)) # publish image message pub.publish(msg) # sleep until next cycle rate.sleep()
def main(): rospy.init_node('capturing', anonymous=True) # initialize the camera and grab a reference to the raw camera capture camera = PiCamera() height = 480 width = 640 camera.resolution = (width, height) camera.framerate = 32 rawCapture = PiRGBArray(camera, size=(width, height)) # allow the camera to warmup time.sleep(0.1) image_pub = rospy.Publisher('image', Image, queue_size=1) image_msg = Image() rate = rospy.Rate(10) # capture frames from the camera for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True): image = frame.array image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) image_msg.header.stamp = rospy.get_rostime() image_msg.header.frame_id = 'raspicam' image_msg.encoding = 'rgb8' image_msg.height = height image_msg.width = width image_msg.step = width * 3 image_msg.data = image.tostring() image_msg.is_bigendian = True image_pub.publish(image_msg) rawCapture.truncate(0) rate.sleep()
def numpy_to_image(arr, encoding): if not encoding in name_to_dtypes: raise TypeError('Unrecognized encoding {}'.format(encoding)) im = Image(encoding=encoding) # extract width, height, and channels dtype_class, exp_channels = name_to_dtypes[encoding] dtype = np.dtype(dtype_class) if len(arr.shape) == 2: im.height, im.width, channels = arr.shape + (1, ) elif len(arr.shape) == 3: im.height, im.width, channels = arr.shape else: raise TypeError("Array must be two or three dimensional") # check type and channels if exp_channels != channels: raise TypeError("Array has {} channels, {} requires {}".format( channels, encoding, exp_channels)) if dtype_class != arr.dtype.type: raise TypeError("Array is {}, {} requires {}".format( arr.dtype.type, encoding, dtype_class)) # make the array contiguous in memory, as mostly required by the format contig = np.ascontiguousarray(arr) im.data = contig.tostring() im.step = contig.strides[0] im.is_bigendian = (arr.dtype.byteorder == '>' or arr.dtype.byteorder == '=' and sys.byteorder == 'big') return im
def copy_Image_empty_data(image_old): image_new = Image() image_new.header = copy(image_old.header) image_new.height = copy(image_old.height) image_new.width = copy(image_old.width) image_new.encoding = copy(image_old.encoding) image_new.is_bigendian = copy(image_old.is_bigendian) image_new.step = copy(image_old.step) return image_new
def publishCombined(self): #Enter Main Loop while not rospy.is_shutdown(): #Convert to Numpy Arrays map = [] for i in range(0, self.numRobots): map.append(numpy.array(self.searchedData[i].data)) combined2 = map[0] if self.numRobots > 1: #Find Minimum of all maps for i in range(1, self.numRobots): combined2 = numpy.minimum(combined2, map[i]) #Pack Occupancy Grid Message mapMsg = OccupancyGrid() mapMsg.header.stamp = rospy.Time.now() mapMsg.header.frame_id = self.mapData.header.frame_id mapMsg.info.resolution = self.mapData.info.resolution mapMsg.info.width = self.mapData.info.width mapMsg.info.height = self.mapData.info.height mapMsg.info.origin = self.mapData.info.origin mapMsg.data = combined2.tolist() #Convert combined Occupancy grid values to grayscal image values combined2[combined2 == -1] = 150 #Unknown -1->150 (gray) combined2[combined2 == 100] = 255 #Not_Searched 100->255 (white) #Searched=0 (black) #Calculate percentage of open area searched numNotSearched = combined2[combined2 == 255].size numSearched = combined2[combined2 == 0].size percentSearched = 100 * float(numSearched) / (numNotSearched + numSearched) percentSearchedMsg = Float32() percentSearchedMsg.data = percentSearched self.percentPub.publish(percentSearchedMsg) #Pack Image Message imageMsg = Image() imageMsg.header.stamp = rospy.Time.now() imageMsg.header.frame_id = self.mapData.header.frame_id imageMsg.height = self.mapData.info.height imageMsg.width = self.mapData.info.width imageMsg.encoding = 'mono8' imageMsg.is_bigendian = 0 imageMsg.step = self.mapData.info.width imageMsg.data = combined2.tolist() #Publish Combined Occupancy Grid and Image self.searchedCombinePub.publish(mapMsg) self.imagePub.publish(imageMsg) #Update Every 0.5 seconds rospy.sleep(1.0)
def record_rgb(object_name): """Recording 2D visuals Spawn the object, record the visuals and delete the object Parameters ---------- object_name : str The name of the tool or object to be recorded """ # Spawn the object rospack = rospkg.RosPack() object_urdf_path = str( rospack.get_path('affordance_experiment') + '/models/' + object_name + '.urdf') pitch = pi / 2 yaw = 0 if object_name == 'cylinder': yaw = pi / 2 spawn_object(object_name, object_urdf_path, 0.0, 0.35, 1.05, pi / 2, pitch, yaw) # Wait for the object to rest rospy.sleep(2) # Get the RGB image from the Kinect sensor # Note: There are issues with wait_for_message not getting the # most up to date message from /camera/rgb/image_raw. # This is a hack to wait for the updated message current_time = rospy.get_rostime() orig_rgb = rospy.wait_for_message('/camera/rgb/image_raw', Image) while orig_rgb.header.stamp < current_time: orig_rgb = rospy.wait_for_message('/camera/rgb/image_raw', Image) # Copy the recevied Image message to a new Image message copy_rgb = Image() copy_rgb.header = orig_rgb.header copy_rgb.height = orig_rgb.height copy_rgb.width = orig_rgb.width copy_rgb.encoding = orig_rgb.encoding copy_rgb.is_bigendian = orig_rgb.is_bigendian copy_rgb.step = orig_rgb.step copy_rgb.data = orig_rgb.data # Publish the new Image to be saved with the specific name rgb_pub = rospy.Publisher(object_name + '_rgb', Image, queue_size=1) rospy.loginfo('-------------------------------------------------') rospy.loginfo('Generating RGB') rospy.loginfo('-------------------------------------------------') rospy.sleep(3) rgb_pub.publish(copy_rgb) # Delete object delete_object(object_name) rospy.sleep(3)
def construct_image(header: Header, response: ImageResponse, encoding: str) -> Image: msg = Image() msg.header = header msg.encoding = encoding msg.height = response.height msg.width = response.width msg.data = response.image_data_uint8 if response.image_type != ImageType.DepthPlanar else response.image_data_float msg.is_bigendian = 0 msg.step = response.width * 3 return msg
def publishCombined(self): #Enter Main Loop while not rospy.is_shutdown(): #Convert to Numpy Arrays map = [] for i in range(0, self.numRobots): map.append(numpy.array(self.searchedData[i].data)) combined2 = map[0] if self.numRobots > 1: #Find Minimum of all maps for i in range(1, self.numRobots): combined2 = numpy.minimum(combined2,map[i]) #Pack Occupancy Grid Message mapMsg=OccupancyGrid() mapMsg.header.stamp=rospy.Time.now() mapMsg.header.frame_id=self.mapData.header.frame_id mapMsg.info.resolution=self.mapData.info.resolution mapMsg.info.width=self.mapData.info.width mapMsg.info.height=self.mapData.info.height mapMsg.info.origin=self.mapData.info.origin mapMsg.data=combined2.tolist() #Convert combined Occupancy grid values to grayscal image values combined2[combined2 == -1] = 150 #Unknown -1->150 (gray) combined2[combined2 == 100] = 255 #Not_Searched 100->255 (white) #Searched=0 (black) #Calculate percentage of open area searched numNotSearched = combined2[combined2==255].size numSearched = combined2[combined2==0].size percentSearched = 100*float(numSearched)/(numNotSearched+numSearched) percentSearchedMsg = Float32() percentSearchedMsg.data = percentSearched self.percentPub.publish(percentSearchedMsg) #Pack Image Message imageMsg=Image() imageMsg.header.stamp = rospy.Time.now() imageMsg.header.frame_id = self.mapData.header.frame_id imageMsg.height = self.mapData.info.height imageMsg.width = self.mapData.info.width imageMsg.encoding = 'mono8' imageMsg.is_bigendian = 0 imageMsg.step = self.mapData.info.width imageMsg.data = combined2.tolist() #Publish Combined Occupancy Grid and Image self.searchedCombinePub.publish(mapMsg) self.imagePub.publish(imageMsg) #Update Every 0.5 seconds rospy.sleep(1.0)
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 genRosDepthImage(resolution): rosImg = Image() # rosImg.data = np_img.flatten().tobytes() rosImg.header.frame_id = "rgbd_camera_link" # rosImg.header.stamp = time_now rosImg.width = resolution[0] # 宽 rosImg.height = resolution[1] # 高 rosImg.step = resolution[0] * 4 rosImg.encoding = "32FC1" rosImg.is_bigendian = 0 return rosImg
def decode_image(self, frame, orig_msg): msg = Image() msg.data = frame.to_rgb().planes[0].to_bytes() msg.width = frame.width msg.height = frame.height msg.step = frame.width * 3 msg.is_bigendian = 0 msg.encoding = 'rgb8' msg.header = Header() msg.header = orig_msg.header self.pub.publish(msg)
def cv2_to_imgmsg(cv_image): img_msg = Image() img_msg.height = cv_image.shape[0] img_msg.width = cv_image.shape[1] img_msg.encoding = "bgr8" img_msg.is_bigendian = 0 img_msg.data = cv_image.tostring() img_msg.step = len( img_msg.data ) // img_msg.height # That double line is actually integer division, not a comment return img_msg
def _publish_img(self, obs): # Hardcoded Implementation of ros_numpy's ImageConverter img_msg = Image(encoding='uint8') img_msg.height, img_msg.width, _ = obs.shape contig = np.ascontiguousarray(obs) img_msg.data = contig.tostring() img_msg.step = contig.strides[0] img_msg.is_bigendian = (obs.dtype.byteorder == '>' or obs.dtype.byteorder == '=' and sys.byteorder == 'big') self.cam_pub.publish(img_msg)
def CreateDepthMessage(img_depth, img_time, sequence): msg_d = Image() bridge_d = CvBridge() msg_d.header.stamp = img_time msg_d.header.seq = sequence msg_d.header.frame_id = "world" msg_d.encoding = "32FC1" msg_d.height = img_depth.shape[0] msg_d.width = img_depth.shape[1] msg_d.data = bridge_d.cv2_to_imgmsg(img_depth, "32FC1").data msg_d.is_bigendian = 0 msg_d.step = msg_d.width * 4 return msg_d
def CreateRGBMessage(img_rgb, img_time, sequence): msg_rgb = Image() bridge_rgb = CvBridge() msg_rgb.header.stamp = img_time msg_rgb.header.seq = sequence msg_rgb.header.frame_id = "world" msg_rgb.encoding = "bgr8" msg_rgb.height = img_rgb.shape[0] msg_rgb.width = img_rgb.shape[1] msg_rgb.data = bridge_rgb.cv2_to_imgmsg(img_rgb, "bgr8").data msg_rgb.is_bigendian = 0 msg_rgb.step = msg_rgb.width * 3 return msg_rgb
def callback(self, data): #print "got some shit coming in" #if data is not None: #plane= data.pose.position #nrm= norm([plane.x, plane.y, plane.z]) #normal= np.array([plane.x, plane.y, plane.z])/nrm #print "got here" ##replace with numpy array #plane= np.array([plane.x, plane.y, plane.z]) ##get the rotation matrix #rmatrix= rotationMatrix(normal) ##print rmatrix ##for point in data.points: ##print point ##p= np.array([point.x, point.y, point.z]) ##flattened_point= project(p, plane, normal) ##print flattened_point ##print np.dot(rmatrix,flattened_point) try: resp= self.seperation() print resp.result #self.pub.publish(resp.clusters[0]) im= Image() im.header.seq= 72 im.header.stamp.secs= 1365037570 im.header.stamp.nsecs= 34077284 im.header.frame_id= '/camera_rgb_optical_frame' im.height= 480 im.width= 640 im.encoding= '16UC1' im.is_bigendian= 0 im.step= 1280 im.data= [100 for n in xrange(1280*480)] for point in resp.clusters[0].points: x= point.x * 640 y= point.y * 480 im.data[y*1280 + x] = 10 pub_image.publish(im) except Exception, e: print "service call failed" print e
def CreateMonoMessage(img_rgb, img_time, sequence): msg_mono = Image() bridge_mono = CvBridge() img_mono = cv2.cvtColor(img_rgb, cv2.COLOR_BGR2GRAY) msg_mono.header.stamp = img_time msg_mono.header.seq = sequence msg_mono.header.frame_id = "world" msg_mono.encoding = "mono8" msg_mono.height = img_rgb.shape[0] msg_mono.width = img_rgb.shape[1] msg_mono.data = bridge_mono.cv2_to_imgmsg(img_mono, "mono8").data msg_mono.is_bigendian = 0 msg_mono.step = msg_mono.width return msg_mono
def map_image(values): """ Map the values generated with the hypothesis-ros image strategy to a rospy Image type. """ if not isinstance(values, _Image): raise TypeError('Wrong type. Use appropriate hypothesis-ros type.') ros_image = Image() ros_image.header = values.header ros_image.height = values.height ros_image.width = values.width ros_image.encoding = values.encoding ros_image.is_bigendian = values.is_bigendian ros_image.data = values.data return ros_image
def _array_to_imgmsg(img_array, encoding): assert len(img_array.shape) == 3 img_msg = Image() img_msg.height = img_array.shape[0] img_msg.width = img_array.shape[1] if encoding == 'passthrough': img_msg.encoding = '8UC3' else: img_msg.encoding = encoding if img_array.dtype.byteorder == '>': img_msg.is_bigendian = True img_msg.data = img_array.tostring() img_msg.step = len(img_msg.data) // img_msg.height return img_msg
def main(): global raw_image_msg rospy.init_node('object_detection') rospy.Subscriber("/raw_image", Image, object_detection_callback) p = pr.Predictor() image_pub = rospy.Publisher("/image", Image, queue_size=10) while raw_image_msg is None: continue azure_addr = "137.135.81.74" while not rospy.is_shutdown(): image_array = list_to_array(raw_image_msg.data, raw_image_msg.height, raw_image_msg.width) try: cv2.imwrite("./input.png", image_array) # upload to azure os.system("scp -r ./input.png azureuser@{}:/home/azureuser".format( azure_addr)) # download from azure os.system("scp azureuser@{}:/home/azureuser/output.png ./".format( azure_addr)) output = cv2.imread("output.png") except Exception as e: print(e) print( "Something went wrong when communicating with azure, trying again" ) continue # print(image_array) # output = p.transform(image_array) # print(numpy.all(output == 0)) rgb_list, h, w = array_to_list(output) # print(rgb_list) image_msg = Image() image_msg.header.stamp = rospy.Time.now() image_msg.header.frame_id = 'a' image_msg.height = h image_msg.width = w image_msg.encoding = 'bgr8' image_msg.is_bigendian = 1 image_msg.step = 3 * w image_msg.data = rgb_list image_pub.publish(image_msg)
def publish_image(self): # get the image from the Nao img = self.nao_cam.getImageRemote(self.proxy_name) # copy the data into the ROS Image ros_img = Image() ros_img.width = img[0] ros_img.height = img[1] ros_img.step = img[2] * img[0] ros_img.header.stamp.secs = img[5] ros_img.data = img[6] ros_img.is_bigendian = False ros_img.encoding = "rgb8" ros_img.data = img[6] # publish the image self.nao_cam_pub.publish(ros_img)
def OccupancyGridToNavImage(self, grid_map): img = Image() img.header = grid_map.header img.height = grid_map.info.height img.width = grid_map.info.width img.is_bigendian = 1 img.step = grid_map.info.width img.encoding = "mono8" maxindex = img.height*img.width numpy.uint for i in range(0, maxindex, 1): if int(grid_map.data[i]) < 20: #img.data[i] = "0" data.append(0) else: img.data[i] = "255" return imgding
def array_to_image(array): """Takes a NxMx3 array and converts it into a ROS image message. """ # Sanity check the input array shape if len(array.shape) != 3 or array.shape[2] != 3: raise ValueError('Array must have shape MxNx3') # Ravel the array into a single buffer image_data = (array.astype(np.uint8)).tostring(order='C') # Create the image message image_msg = Image() image_msg.height = array.shape[0] image_msg.width = array.shape[1] image_msg.encoding = 'rgb8' image_msg.is_bigendian = 0 image_msg.step = array.shape[1] * 3 image_msg.data = image_data return image_msg
def main(): pub = rospy.Publisher('image_maker', Image) rospy.init_node('image_maker') #rospy.wait_for_service('tabletop_segmentation') im= Image() im.header.seq= 72 im.header.stamp.secs= 1365037570 im.header.stamp.nsecs= 34077284 im.header.frame_id= '/camera_rgb_optical_frame' im.height= 480 im.width= 640 im.encoding= '16UC1' im.is_bigendian= 0 im.step= 1280 im.data= [100 for n in xrange(1280*480)] while not rospy.is_shutdown(): try: pub.publish(im) sleep(.5) except Exception, e: print e
def numpy_to_image(arr, encoding): if not encoding in name_to_dtypes: raise TypeError('Unrecognized encoding {}'.format(encoding)) im = Image(encoding=encoding) # extract width, height, and channels dtype_class, exp_channels = name_to_dtypes[encoding] dtype = np.dtype(dtype_class) if len(arr.shape) == 2: im.height, im.width, channels = arr.shape + (1,) elif len(arr.shape) == 3: im.height, im.width, channels = arr.shape else: raise TypeError("Array must be two or three dimensional") # check type and channels if exp_channels != channels: raise TypeError("Array has {} channels, {} requires {}".format( channels, encoding, exp_channels )) if dtype_class != arr.dtype.type: raise TypeError("Array is {}, {} requires {}".format( arr.dtype.type, encoding, dtype_class )) # make the array contiguous in memory, as mostly required by the format contig = np.ascontiguousarray(arr) im.data = contig.tostring() im.step = contig.strides[0] im.is_bigendian = ( arr.dtype.byteorder == '>' or arr.dtype.byteorder == '=' and sys.byteorder == 'big' ) return im
def find_object(self): with self.lock_depth: image_data = self.image_data depth_data = self.depth_data pub_data = rospy.Publisher('/racecar/image/data', Float64MultiArray, queue_size=1) arrayData = Float64MultiArray() #print "received image" if image_data: newImage = Image() newImage.encoding = image_data.encoding newImage.header = image_data.header newImage.is_bigendian = image_data.is_bigendian newData = bytearray() image = image_data.data clusters = [] white = chr(255) *3 black = chr(0) * 3 red = chr(0) + chr(0) + chr(255) blue = chr(255) + chr(0) * 2 #defining color search thresholds redThreshold = 230 blueThreshold = 20 greenLowerThreshold = 50 greenUpperThreshold = 200 height = 0 starttime = time.time() for h in xrange(0, image_data.height, self.resolution): height += 1 for w in xrange(0, image_data.width, self.resolution): index = w * 3 + image_data.width * 3 * h b = ord(image[index]) g = ord(image[index + 1]) r = ord(image[index + 2]) #filtering for the color if r > redThreshold and b < blueThreshold and g <= greenUpperThreshold and g >= greenLowerThreshold: for char in white: newData.append(char) inCluster = False for cluster in clusters: if self.checkAdjacency((w, h), cluster): cluster.add((w, h)) inCluster = True if not inCluster: newCluster = set() newCluster.add((w, h)) clusters.append(newCluster) else: for char in black: newData.append(char) newImage.height = height newImage.width = len(newData)/height/3 newImage.step = len(newData)/height newImage.data = str(newData) #print 'time taken: ', time.time() - starttime clusters = self.reduceClusters(clusters) #print len(clusters), float(len(candidates_x))/len(newData) #print [len(cluster) for cluster in clusters] if len(clusters) > 1: print "multiple objects found" print [len(cluster) for cluster in clusters] if len(clusters): publishableClusters = [max(clusters, key=lambda x:len(x))] data_to_publish = [] for cluster in publishableClusters: centerX = sum(x for (x, y) in cluster)/len(cluster) centerY = sum(y for (x, y) in cluster)/len(cluster) width = max(cluster, key=lambda (x, y):x)[0] - min(cluster, key=lambda (x, y):x)[0] height = max(cluster, key=lambda (x, y):y)[1] - min(cluster, key=lambda (x, y):y)[1] relativeLocation = float(centerX)/image_data.width - 0.5 relativeAngle = relativeLocation * 100 * math.pi / 180 data_to_publish += [centerX, centerY, width, height, relativeAngle] #print 'center ', centerX, centerY, len(candidates_x) #print 'distance ', depth1, depth2 #print 'dim ', height, width '''depth_index = centerX * 2 + image_data.width * 2 * centerY depth1 = ord(depth_data.data[depth_index]) depth2 = ord(depth_data.data[depth_index + 1])''' arrayData.data = data_to_publish else: arrayData.data = [-1, -1, -1, -1, -1] print 'no cone found' pub_data.publish(arrayData) #print "publishing" pub_img = rospy.Publisher('/racecar/image/newImage', Image, queue_size=1) pub_img.publish(newImage) else: arrayData.data = [-1, -1, -1, -1] print 'image not ready'
def spin(self): while not rospy.is_shutdown(): m = MarkerArray() stamp, f1_tact, f2_tact, f3_tact, palm_tact = self.bh.getTactileData() finger_skin_data = [] finger_skin_data.append(f1_tact) finger_skin_data.append(f2_tact) finger_skin_data.append(f3_tact) finger_skin_data.append(palm_tact) frame_id = [] frame_id.append(self.prefix+"_HandFingerOneKnuckleThreeLink") frame_id.append(self.prefix+"_HandFingerTwoKnuckleThreeLink") frame_id.append(self.prefix+"_HandFingerThreeKnuckleThreeLink") frame_id.append(self.prefix+"_HandPalmLink") arrows = False if arrows: for sens in range(0, 4): for i in range(0, 24): halfside1 = PyKDL.Vector(pressure_info.sensor[sens].halfside1[i].x, pressure_info.sensor[sens].halfside1[i].y, pressure_info.sensor[sens].halfside1[i].z) halfside2 = PyKDL.Vector(pressure_info.sensor[sens].halfside2[i].x, pressure_info.sensor[sens].halfside2[i].y, pressure_info.sensor[sens].halfside2[i].z) # calculate cross product: halfside1 x halfside2 norm = halfside1*halfside2 norm.Normalize() scale = 0 if sens == 0: scale = data.finger1_tip[i]/256.0 elif sens == 1: scale = data.finger2_tip[i]/256.0 elif sens == 2: scale = data.finger3_tip[i]/256.0 else: scale = data.palm_tip[i]/256.0 norm = norm*scale*0.01 marker = Marker() marker.header.frame_id = frame_id[sens] marker.header.stamp = rospy.Time.now() marker.ns = frame_id[sens] marker.id = i marker.type = 0 marker.action = 0 marker.points.append(Point(pressure_info.sensor[sens].center[i].x,pressure_info.sensor[sens].center[i].y,pressure_info.sensor[sens].center[i].z)) marker.points.append(Point(pressure_info.sensor[sens].center[i].x+norm.x(), pressure_info.sensor[sens].center[i].y+norm.y(), pressure_info.sensor[sens].center[i].z+norm.z())) marker.pose = Pose( Point(0,0,0), Quaternion(0,0,0,1) ) marker.scale = Vector3(0.001, 0.002, 0) marker.color = ColorRGBA(1,0,0,1) m.markers.append(marker) self.pub.publish(m) else: for sens in range(0, 4): for i in range(0, 24): value = self.convertToRGB(int(finger_skin_data[sens][i]/2)) marker = Marker() marker.header.frame_id = frame_id[sens] marker.header.stamp = rospy.Time.now() marker.ns = frame_id[sens] marker.id = i marker.type = 1 marker.action = 0 if sens < 3: marker.pose = pm.toMsg(self.bh.pressure_frames[i]) marker.scale = Vector3(self.bh.pressure_cells_size[i][0], self.bh.pressure_cells_size[i][1], 0.004) else: marker.pose = pm.toMsg(self.bh.palm_pressure_frames[i]) marker.scale = Vector3(self.bh.palm_pressure_cells_size[i][0], self.bh.palm_pressure_cells_size[i][1], 0.004) marker.color = ColorRGBA(1.0/255.0*value[0], 1.0/255.0*value[1], 1.0/255.0*value[2],1) m.markers.append(marker) self.pub.publish(m) # palm f1 f2 f3 # xxx xxx xxx # xxx xxx xxx # xxxxx xxx xxx xxx # xxxxxxx xxx xxx xxx # 56789xx 9xx 9xx 9xx # 01234 678 678 678 # 345 345 345 # 012 012 012 im = Image() im.height = 8 im.width = 19 im.encoding = "rgb8" im.is_bigendian = 0 im.step = im.width*3 im.data = [0]*(im.step*im.height) for finger in range(0, 3): for y in range(0, 8): for x in range(0, 3): xim = 8 + x + finger * 4 yim = im.height-1-y value = self.convertToRGB(int(finger_skin_data[finger][y*3+x]/2)) im.data[(yim*im.width + xim)*3+0] = value[0] im.data[(yim*im.width + xim)*3+1] = value[1] im.data[(yim*im.width + xim)*3+2] = value[2] palm_im_x = [1, 2, 3, 4, 5, 0, 1, 2, 3, 4, 5, 6, 0, 1, 2, 3, 4, 5, 6, 1, 2, 3, 4, 5] palm_im_y = [5, 5, 5, 5, 5, 4, 4, 4, 4, 4, 4, 4, 3, 3, 3, 3, 3, 3, 3, 2, 2, 2, 2, 2] for i in range(0, 24): xim = palm_im_x[i] yim = palm_im_y[i] value = self.convertToRGB(int(finger_skin_data[3][i]/2)) im.data[(yim*im.width + xim)*3+0] = value[0] im.data[(yim*im.width + xim)*3+1] = value[1] im.data[(yim*im.width + xim)*3+2] = value[2] self.tactileImagepub.publish(im) rospy.sleep(0.1)
UDK_camera=(240,320) #box(left, upper, right, lower) box_A = (0, 0, UDK_camera[1],UDK_camera[0]) box_B = (UDK_camera[1],0,UDK_camera[1]*2,UDK_camera[0]) box_C = (UDK_camera[1]*2,0,UDK_camera[1]*3,UDK_camera[0]) box_D = (UDK_camera[1]*3,0,UDK_camera[1]*4,UDK_camera[0]) clientsock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) clientsock.connect((host,port)) c_msg = 'OK\r\n' clientsock.sendall(c_msg) #set image info img_A=Image() img_A.encoding='rgb8' img_A.is_bigendian =0 img_A.step =1 img_A.height = UDK_camera[0] img_A.width = UDK_camera[1] img_B=Image() img_B.encoding='rgb8' img_B.is_bigendian =0 img_B.step =1 img_B.height = UDK_camera[0] img_B.width = UDK_camera[1] img_C=Image() img_C.encoding='rgb8' img_C.is_bigendian =0 img_C.step =1