def CreateMonoBag(imgs,bagname):
'''Creates a bag file with camera images'''
bag = rosbag.Bag(bagname, 'w')
imgs = sorted(imgs)
try:
for i in range(len(imgs)):
print("Adding %s" % imgs[i])
fp = open( imgs[i], "r" )
p = ImageFile.Parser()
rgb_file = imgs[i]
llim = rgb_file.rfind('/')
rlim = rgb_file.rfind('.')
rgb_ext = rgb_file[rlim:]
msec = rgb_file[llim+1:rlim]
sec = float(msec) / 1000 # msec to sec
while 1:
s = fp.read(1024)
if not s:
break
p.feed(s)
im = p.close()
# Stamp = rospy.rostime.Time.from_sec(time.time())
Stamp = rospy.rostime.Time.from_sec(sec)
Img = Image()
Img.header.stamp = Stamp
Img.width = im.size[0]
Img.height = im.size[1]
Img.encoding = "rgb8"
Img.header.frame_id = "camera"
Img_data = [pix for pixdata in im.getdata() for pix in pixdata]
Img.data = Img_data
bag.write('camera/rgb/image_color', Img, Stamp)
#####
d_file = rgb_file.replace(rgb_ext, '.txt')
print("Adding %s" % d_file)
fid = open(d_file, 'r')
raw = fid.readlines()
fid.close()
#depth = numpy.reshape(raw, (im.size[1], im.size[0]))
Img_depth = Image()
Img_depth.header.stamp = Stamp
Img_depth.width = im.size[0]
Img_depth.height = im.size[1]
Img_depth.encoding = "rgb8"
Img_depth.header.frame_id = "camera"
#Img_data = [pix for pixdata in im.getdata() for pix in pixdata]
Img_depth.data = raw
bag.write('camera/depth/image', Img, Stamp)
finally:
bag.close()
def CreateStereoBag(left_imgs, right_imgs, bagname):
'''Creates a bag file containing stereo image pairs'''
bag =rosbag.Bag(bagname, 'w')
try:
for i in range(len(left_imgs)):
print("Adding %s" % left_imgs[i])
fp_left = open( left_imgs[i], "r" )
p_left = ImageFile.Parser()
while 1:
s = fp_left.read(1024)
if not s:
break
p_left.feed(s)
im_left = p_left.close()
fp_right = open( right_imgs[i], "r" )
print("Adding %s" % right_imgs[i])
p_right = ImageFile.Parser()
while 1:
s = fp_right.read(1024)
if not s:
break
p_right.feed(s)
im_right = p_right.close()
Stamp = roslib.rostime.Time.from_sec(time.time())
Img_left = Image()
Img_left.header.stamp = Stamp
Img_left.width = im_left.size[0]
Img_left.height = im_left.size[1]
Img_left.encoding = "rgb8"
Img_left.header.frame_id = "camera/left"
Img_left_data = [pix for pixdata in im_left.getdata() for pix in pixdata]
Img_left.data = Img_left_data
Img_right = Image()
Img_right.header.stamp = Stamp
Img_right.width = im_right.size[0]
Img_right.height = im_right.size[1]
Img_right.encoding = "rgb8"
Img_right.header.frame_id = "camera/right"
Img_right_data = [pix for pixdata in im_right.getdata() for pix in pixdata]
Img_right.data = Img_right_data
bag.write('camera/left/image_raw', Img_left, Stamp)
bag.write('camera/right/image_raw', Img_right, Stamp)
finally:
bag.close()
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 main_loop(self):
img = Image()
while not rospy.is_shutdown():
#print "getting image..",
image = self.camProxy.getImageRemote(self.nameId)
#print "ok"
# TODO: better time
img.header.stamp = rospy.Time.now()
img.header.frame_id = self.frame_id
img.height = image[1]
img.width = image[0]
nbLayers = image[2]
#colorspace = image[3]
if image[3] == kYUVColorSpace:
encoding = "mono8"
elif image[3] == kRGBColorSpace:
encoding = "rgb8"
elif image[3] == kBGRColorSpace:
encoding = "bgr8"
else:
rospy.logerror("Received unknown encoding: {0}".format(image[3]))
img.encoding = encoding
img.step = img.width * nbLayers
img.data = image[6]
self.info_.width = img.width
self.info_.height = img.height
self.info_.header = img.header
self.pub_img_.publish(img)
self.pub_info_.publish(self.info_)
rospy.sleep(0.0001)# TODO: is this necessary?
self.camProxy.unsubscribe(self.nameId)
def post_image(self, component_instance):
""" Publish the data of the Camera as a ROS-Image message.
"""
image_local = component_instance.local_data['image']
if not image_local or image_local == '' or not image_local.image or not component_instance.capturing:
return # press [Space] key to enable capturing
parent_name = component_instance.robot_parent.blender_obj.name
image = Image()
image.header.stamp = rospy.Time.now()
image.header.seq = self._seq
# http://www.ros.org/wiki/geometry/CoordinateFrameConventions#Multi_Robot_Support
image.header.frame_id = ('/' + parent_name + '/base_image')
image.height = component_instance.image_height
image.width = component_instance.image_width
image.encoding = 'rgba8'
image.step = image.width * 4
# NOTE: Blender returns the image as a binary string encoded as RGBA
# sensor_msgs.msg.Image.image need to be len() friendly
# TODO patch ros-py3/common_msgs/sensor_msgs/src/sensor_msgs/msg/_Image.py
# to be C-PyBuffer "aware" ? http://docs.python.org/c-api/buffer.html
image.data = bytes(image_local.image)
# RGBA8 -> RGB8 ? (remove alpha channel, save h*w bytes, CPUvore ?)
# http://wiki.blender.org/index.php/Dev:Source/GameEngine/2.49/VideoTexture
# http://www.blender.org/documentation/blender_python_api_2_57_release/bge.types.html#bge.types.KX_Camera.useViewport
for topic in self._topics:
# publish the message on the correct topic
if str(topic.name) == str("/" + parent_name + "/" + component_instance.blender_obj.name):
topic.publish(image)
self._seq = self._seq + 1
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 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 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 CreateMonoBag(imgs,bagname):
'''Creates a bag file with camera images'''
bag =rosbag.Bag(bagname, 'w')
try:
for i in range(len(imgs)):
print("Adding %s" % imgs[i])
fp = open( imgs[i], "r" )
p = ImageFile.Parser()
while 1:
s = fp.read(1024)
if not s:
break
p.feed(s)
im = p.close()
Stamp = rospy.rostime.Time.from_sec(time.time())
Img = Image()
Img.header.stamp = Stamp
Img.width = im.size[0]
Img.height = im.size[1]
Img.encoding = "rgb8"
Img.header.frame_id = "camera"
Img_data = [pix for pixdata in im.getdata() for pix in pixdata]
Img.data = Img_data
bag.write('camera/image_raw', Img, Stamp)
finally:
bag.close()
def main_loop(self):
img = Image()
r = rospy.Rate(self.fps)
while not rospy.is_shutdown():
image = self.camProxy.getImageRemote(self.nameId)
stampAL = image[4] + image[5]*1e-6
#print image[5], stampAL, "%lf"%(stampAL)
img.header.stamp = rospy.Time(stampAL)
img.header.frame_id = self.frame_id
img.height = image[1]
img.width = image[0]
nbLayers = image[2]
if image[3] == kYUVColorSpace:
encoding = "mono8"
elif image[3] == kRGBColorSpace:
encoding = "rgb8"
elif image[3] == kBGRColorSpace:
encoding = "bgr8"
elif image[3] == kYUV422ColorSpace:
encoding = "yuv422" # this works only in ROS groovy and later
else:
rospy.logerror("Received unknown encoding: {0}".format(image[3]))
img.encoding = encoding
img.step = img.width * nbLayers
img.data = image[6]
infomsg = self.cim.getCameraInfo()
infomsg.header = img.header
self.pub_info_.publish(infomsg)
self.pub_img_.publish(img)
r.sleep()
self.camProxy.unsubscribe(self.nameId)
def numpy_to_imgmsg(image, stamp=None):
from sensor_msgs.msg import Image
rosimage = Image()
rosimage.height = image.shape[0]
rosimage.width = image.shape[1]
if image.dtype == np.uint8:
rosimage.encoding = '8UC%d' % image.shape[2]
rosimage.step = image.shape[2] * rosimage.width
rosimage.data = image.ravel().tolist()
else:
rosimage.encoding = '32FC%d' % image.shape[2]
rosimage.step = image.shape[2] * rosimage.width * 4
rosimage.data = np.array(image.flat, dtype=np.float32).tostring()
if stamp is not None:
rosimage.header.stamp = stamp
return rosimage
def __init__(self):
rospy.init_node('image_publish')
name = sys.argv[1]
image = cv2.imread(name)
#cv2.imshow("im", image)
#cv2.waitKey(5)
hz = rospy.get_param("~rate", 1)
frame_id = rospy.get_param("~frame_id", "map")
use_image = rospy.get_param("~use_image", True)
rate = rospy.Rate(hz)
self.ci_in = None
ci_sub = rospy.Subscriber('camera_info_in', CameraInfo,
self.camera_info_callback, queue_size=1)
if use_image:
pub = rospy.Publisher('image', Image, queue_size=1)
ci_pub = rospy.Publisher('camera_info', CameraInfo, queue_size=1)
msg = Image()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = frame_id
msg.encoding = 'bgr8'
msg.height = image.shape[0]
msg.width = image.shape[1]
msg.step = image.shape[1] * 3
msg.data = image.tostring()
if use_image:
pub.publish(msg)
ci = CameraInfo()
ci.header = msg.header
ci.height = msg.height
ci.width = msg.width
ci.distortion_model ="plumb_bob"
# TODO(lucasw) need a way to set these values- have this node
# subscribe to an input CameraInfo?
ci.D = [0.0, 0.0, 0.0, 0, 0]
ci.K = [500.0, 0.0, msg.width/2, 0.0, 500.0, msg.height/2, 0.0, 0.0, 1.0]
ci.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
ci.P = [500.0, 0.0, msg.width/2, 0.0, 0.0, 500.0, msg.height/2, 0.0, 0.0, 0.0, 1.0, 0.0]
# ci_pub.publish(ci)
# TODO(lwalter) only run this is hz is positive,
# otherwise wait for input trigger message to publish an image
while not rospy.is_shutdown():
if self.ci_in is not None:
ci = self.ci_in
msg.header.stamp = rospy.Time.now()
ci.header = msg.header
if use_image:
pub.publish(msg)
ci_pub.publish(ci)
if hz <= 0:
rospy.sleep()
else:
rate.sleep()
def process_frame(self,cam_id,buf,buf_offset,timestamp,framenumber):
if have_ROS:
msg = Image()
msg.header.seq=framenumber
msg.header.stamp=rospy.Time.from_sec(timestamp)
msg.header.frame_id = "0"
npbuf = np.array(buf)
(height,width) = npbuf.shape
msg.height = height
msg.width = width
msg.encoding = self.encoding
msg.step = width
msg.data = npbuf.tostring() # let numpy convert to string
with self.publisher_lock:
cam_info = self.camera_info
cam_info.header.stamp = msg.header.stamp
cam_info.header.seq = msg.header.seq
cam_info.header.frame_id = msg.header.frame_id
cam_info.width = width
cam_info.height = height
self.publisher.publish(msg)
self.publisher_cam_info.publish(cam_info)
return [],[]
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 default(self, ci="unused"):
if not self.component_instance.capturing:
return # press [Space] key to enable capturing
image_local = self.data["image"]
image = Image()
image.header = self.get_ros_header()
image.header.frame_id += "/base_image"
image.height = self.component_instance.image_height
image.width = self.component_instance.image_width
image.encoding = "rgba8"
image.step = image.width * 4
# VideoTexture.ImageRender implements the buffer interface
image.data = bytes(image_local)
# fill this 3 parameters to get correcty image with stereo camera
Tx = 0
Ty = 0
R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
intrinsic = self.data["intrinsic_matrix"]
camera_info = CameraInfo()
camera_info.header = image.header
camera_info.height = image.height
camera_info.width = image.width
camera_info.distortion_model = "plumb_bob"
camera_info.K = [
intrinsic[0][0],
intrinsic[0][1],
intrinsic[0][2],
intrinsic[1][0],
intrinsic[1][1],
intrinsic[1][2],
intrinsic[2][0],
intrinsic[2][1],
intrinsic[2][2],
]
camera_info.R = R
camera_info.P = [
intrinsic[0][0],
intrinsic[0][1],
intrinsic[0][2],
Tx,
intrinsic[1][0],
intrinsic[1][1],
intrinsic[1][2],
Ty,
intrinsic[2][0],
intrinsic[2][1],
intrinsic[2][2],
0,
]
self.publish(image)
self.topic_camera_info.publish(camera_info)
def main_loop(self):
img = Image()
while not rospy.is_shutdown():
#print "getting image..",
images = self.camProxy.getImagesRemote (self.nameId)
#print "ok"
# TODO: better time
for i in [0,1]:
#print len(images[i])
image = images[i]
img.header.stamp = rospy.Time.now()
if image[7] == 0:
img.header.frame_id = "/CameraTop_frame"
elif image[7] == 1:
img.header.frame_id = "/CameraBottom_frame"
img.height = image[1]
img.width = image[0]
nbLayers = image[2]
#colorspace = image[3]
if image[3] == kYUVColorSpace:
encoding = "mono8"
elif image[3] == kRGBColorSpace:
encoding = "rgb8"
elif image[3] == kBGRColorSpace:
encoding = "bgr8"
else:
rospy.logerror("Received unknown encoding: {0}".format(image[3]))
img.encoding = encoding
img.step = img.width * nbLayers
if len(images) >= 2:
img.data = images[2][len(images[2])/2 * i:len(images[2])/2 *(i+1) + 1]
else:
img.data = []
print "image with no data"
self.info_[i].width = img.width
self.info_[i].height = img.height
self.info_[i].header = img.header
self.pub_img_[i].publish(img)
self.pub_info_[i].publish(self.info_[i])
self.camProxy.releaseImages(self.nameId)
self.camProxy.unsubscribe(self.nameId)
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 __data_to_image(self, data):
"""
Transforms input state format to Image msg, displayable in rviz.
:param data: input state
"""
msg = Image()
msg.header.frame_id = "/base_footprint"
msg.height = data.shape[1]
msg.width = data.shape[0]
msg.encoding = "mono8"
msg.data = np.uint8(np.ndarray.flatten(data, order='F'))[::-1].tolist()
return msg
def shot(self, data=None):
img_to_send = Image()
image = self.vision.getImageRemote(self.nameId)
img_to_send.header.stamp = rospy.Time.now()
img_to_send.height = image[1]
img_to_send.width = image[0]
layers = image[2] #3 for RGB
img_to_send.encoding = "8UC3" #8 unsigned bit 3 channel
img_to_send.step = img_to_send.width * layers
img_to_send.data = image[6]
rospy.loginfo('Sending image...')
return ShotResponse(img_to_send)
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 write(self, data):
# Publish raw image
data_y = data[:RES[0]*RES[1]]
msg = Image()
msg.header.stamp = rospy.Time.now()
msg.width = RES[0]
msg.height = RES[1]
msg.encoding = "mono8"
msg.step = len(data_y) // RES[1]
msg.data = data_y
self.pub_img.publish(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 parse_seg(self, seg):
# seg.convert(cc.CityScapesPalette)
array = np.frombuffer(seg.raw_data, dtype=np.dtype("uint8")).copy()
not_road = array != 7
array[not_road] = 0
array[~not_road] = 255
img_to_publish = Image()
img_to_publish.data = array.tolist()
img_to_publish.encoding = 'bgra8'
img_to_publish.width = seg.width
img_to_publish.height = seg.height
self.seg_pub.publish(img_to_publish)
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 image_callback(self, img_msg):
a = datetime.now()
n_channels = 3
dtype = 'uint8'
img_buf = np.asarray(img_msg.data, dtype=dtype)
image_np = np.ndarray(shape=(img_msg.height, img_msg.width,
n_channels),
dtype=dtype,
buffer=img_buf)
# Expand dimensions since the model expects images to have shape: [1, None, None, 3]
image_np_expanded = np.expand_dims(image_np, axis=0)
# Actual detection.
(boxes, scores, classes, num) = self.session.run(
[
self.detection_boxes, self.detection_scores,
self.detection_classes, self.num_detections
],
feed_dict={self.image_tensor: image_np_expanded})
# Visualization of the results of a detection.
vis_util.visualize_boxes_and_labels_on_image_array(
image_np,
np.squeeze(boxes),
np.squeeze(classes).astype(np.int32),
np.squeeze(scores),
category_index,
use_normalized_coordinates=True,
line_thickness=8)
# remove additional dimension
classes = classes[0]
scores = scores[0]
num = num[0]
b = datetime.now()
c = b - a
self.get_logger().info("handle_classify_image_srv took: %r" % c)
img_msg = ImageMsg()
img_msg.height = image_np.shape[0]
img_msg.width = image_np.shape[1]
img_msg.encoding = "bgr8"
img_msg.data = image_np.tostring()
img_msg.step = len(img_msg.data) // img_msg.height
img_msg.header.frame_id = "world"
self.pub.publish(img_msg)
def image_process(image, params):
#bridge = CvBridge()
lower = params['lowerY']
upper = params['upperY']
debug_info = params['debug_info']
global error
try:
#cv_image = bridge.imgmsg_to_cv2(image)
raw_data = np.fromstring(image.data, np.uint8)
cv_image = cv2.imdecode(raw_data, cv2.IMREAD_COLOR)
gray = cv2.imdecode(raw_data, cv2.IMREAD_GRAYSCALE)
_, gray = cv2.threshold(gray, 250, 255, cv2.THRESH_BINARY)
y_len, x_len = gray.shape
lower, upper = max(0, lower), min(upper, y_len)
if debug_info: print(lower, upper, 'image crop in Y')
gray = gray[lower:upper, :]
y_len, x_len = gray.shape
total_error = []
for y in range(10, y_len, 90):
weighted_mass = 0
mass = 0
for x in range(0, x_len):
if gray[y, x] == 255:
mass += 1
weighted_mass += x
if mass > 0:
final_x = int(weighted_mass / mass)
total_error.append(
float(final_x - x_len // 2) / float(x_len // 2))
if params['display_processed_image']:
cv2.rectangle(gray, (final_x - 10, y),
(final_x + 10, y + 35), 120, 2)
#cv2.imshow('wooho', gray)
if debug_info: print(total_error)
if len(total_error) > 2:
error = sum(total_error) / len(total_error)
if params['display_image']: cv2.imshow('raw', cv_image)
if params['display_processed_image']: cv2.imshow('processed', gray)
if params['publish_processed_image']:
msg = Image()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', gray)[1]).tostring()
params['img_pub'].publish(msg)
cv2.waitKey(1)
except Exception as e:
if debug_info:
print(e)
def read_cam(self):
cap = cv2.VideoCapture(
"nvarguscamerasrc ! video/x-raw(memory:NVMM), width=(int)640, height=(int)360,format=(string)NV12, framerate=(fraction)7/1 ! nvvidconv ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR ! appsink"
)
#print(cap.isOpened())
if cap.isOpened():
#cv2.namedWindow("demo", cv2.WINDOW_AUTOSIZE)
while not rospy.is_shutdown():
ret_val, cv_image = cap.read()
cv_image = cv2.flip(cv_image, -1)
#print('Shape',cv_image.shape)
# cv2.imshow('demo',cv_image)
#print("cv_image", cv_image)
try:
#bridge = CvBridge().cv2_to_imgmsg(cv_image, "bgr8")
#self.image_pub.publish(bridge)
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg',
cv_image)[1]).tostring()
#### Create RawImage ####
msg_raw = Image()
msg_raw.header.stamp = rospy.Time.now()
msg_raw.encoding = "bgr8"
msg_raw.height = 360
msg_raw.width = 640
msg_raw.data = np.array(cv_image).tostring()
msg_raw.step = 640 * 3
# Publish new image
self.image_pub.publish(msg)
self.image_pub_raw.publish(msg_raw)
except CvBridgeError as e:
print(e)
keypressed = cv2.waitKey(1) % 256
#print("key ",keypressed)
if keypressed == 27:
print("escape key pressed")
cv2.destroyAllWindows()
break
else:
print("camera open failed, retrying")
self.read_cam()
def publish_image(self, image):
img = Image()
img.encoding = 'rgb8'
img.width = 416
img.height = 320
img.step = img.width * 3
img.data = image
img.header.frame_id = 'raspicam'
img.header.stamp = self._get_stamp(time())
self._img_pub.publish(img)
camera_info = self._camera_info_manager.getCameraInfo()
camera_info.header = img.header
self._camera_info_pub.publish(camera_info)
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 publish_image(imgdata):
image_temp = Image()
header = Header(stamp=rospy.Time.now())
header.frame_id = 'map'
image_temp.height = IMAGE_HEIGHT
image_temp.width = IMAGE_WIDTH
image_temp.encoding = 'rgb8'
image_temp.data = np.array(imgdata).tostring()
#print(imgdata)
#image_temp.is_bigendian=True
image_temp.header = header
image_temp.step = 1241 * 3
image_pub.publish(image_temp)
def publish_image(imgdata):
image_temp = Image()
header = Header(stamp=rospy.Time.now())
header.frame_id = 'map'
image_temp.height = np.shape(imgdata)[0]
image_temp.width = np.shape(imgdata)[1]
image_temp.encoding = 'rgb8'
image_temp.data = np.array(imgdata).tostring()
#print(imgdata)
#image_temp.is_bigendian=True
image_temp.header = header
image_temp.step = np.shape(imgdata)[1] * 3
img_pub.publish(image_temp)
def write_img(ts_ns, img_array, camera, bag):
ros_timestamp = nanoseconds_to_ros_timestamp(ts_ns)
rosimage = Image()
rosimage.data = img_array.tostring()
rosimage.step = img_array.shape[
1] #only with mono8! (step = width * byteperpixel * numChannels)
rosimage.encoding = "mono8"
rosimage.height = img_array.shape[0]
rosimage.width = img_array.shape[1]
rosimage.header.stamp = ros_timestamp
bag.write('/' + camera + '/image_raw', rosimage, t=ros_timestamp)
def pubImage(publisher, nd_image):
out_msg = Image()
out_msg.height = nd_image.shape[0]
out_msg.width = nd_image.shape[1]
# print(out_vis_alpha_msg.height, out_vis_alpha_msg.width)
out_msg.step = nd_image.strides[0]
out_msg.encoding = 'bgr8'
out_msg.header.frame_id = 'map'
out_msg.header.stamp = rospy.Time.now()
out_msg.data = nd_image.flatten().tolist()
publisher.publish(out_msg)
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 _create_image_and_info_messages(self, image):
image_msg = Image()
image_msg.height = image.shape[0]
image_msg.width = image.shape[1]
image_msg.encoding = 'bgr8'
image_msg.step = image_msg.width * 3
image_msg.data = array.array('B', image.tobytes())
camera_info_msg = CameraInfo()
camera_info_msg.height = image.shape[0]
camera_info_msg.width = image.shape[1]
return (image_msg, camera_info_msg)
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 CreateMonoBag(imgs, bagname, timestamps):
'''read time stamps'''
file = open(timestamps, 'r')
timestampslines = file.readlines()
file.close()
'''Creates a bag file with camera images'''
bag =rosbag.Bag(bagname, 'w')
try:
for i in range(len(imgs)):
print("Adding %s" % imgs[i])
fp = open( imgs[i], "r" )
p = ImageFile.Parser()
'''read image size'''
imgpil = ImagePIL.open(imgs[0])
width, height = imgpil.size
# print "size:",width,height
while 1:
s = fp.read(1024)
if not s:
break
p.feed(s)
im = p.close()
Stamp = rospy.rostime.Time.from_sec(float(timestampslines[i]))
'''set image information '''
Img = Image()
Img.header.stamp = Stamp
Img.height = height
Img.width = width
Img.header.frame_id = "camera"
'''for rgb8'''
# Img.encoding = "rgb8"
# Img_data = [pix for pixdata in im.getdata() for pix in pixdata]
# Img.step = Img.width * 3
'''for mono8'''
Img.encoding = "mono8"
Img_data = [pix for pixdata in [im.getdata()] for pix in pixdata]
Img.step = Img.width
Img.data = Img_data
bag.write('camera/image_raw', Img, Stamp)
finally:
bag.close()
def _publish_image(self):
camera_image = self._cozmo.world.latest_image
if camera_image is not None:
img = camera_image.raw_image
ros_img = Image()
ros_img.encoding = 'rgb8'
ros_img.width = img.size[0]
ros_img.height = img.size[1]
ros_img.step = 3 * ros_img.width
ros_img.data = img.tobytes()
ros_img.header.frame_id = 'cozmo_camera'
cozmo_time = camera_image.image_recv_time
ros_img.header.stamp = rospy.Time.from_sec(cozmo_time)
self._image_pub.publish(ros_img)
def write(self, buf):
img_msg = Image()
img_msg.height = self.height
img_msg.width = self.width
img_msg.encoding = "rgb8" # "bgr8"?
img_msg.is_bigendian = False # not sure about this either
img_msg.data = buf
img_msg.step = 3 * self.width
#print("publishing image message")
self.publisher.publish(img_msg)
def make_rgb_msg(rgb, rgb_time_sec, rgb_time_nsec, test_image):
rgb_msg = Image()
rgb_msg.header.seq = test_image
rgb_msg.header.frame_id = "/openni_rgb_optical_frame"
rgb_msg.header.stamp.secs = rgb_time_sec
rgb_msg.header.stamp.nsecs = rgb_time_nsec
rgb_msg.step = 1920
rgb_msg.is_bigendian = 0
rgb_msg.encoding = 'rgb8'
rgb_msg.data = rgb
return rgb_msg
def toROS(img):
"""
Convert a PIL/pygame image to a ROS compatible message (sensor_msgs.Image).
"""
if img.mode == 'P': # P -> 8-bit pixels, mapped to any other mode using a color palette
img = img.convert('RGB') # RGB -> 3x8-bit pixels, true color
rosimage = ImageMsg()
rosimage.encoding = ImageConverter.ENCODINGMAP_PY_TO_ROS[img.mode]
(rosimage.width, rosimage.height) = img.size
rosimage.step = (ImageConverter.PIL_MODE_CHANNELS[img.mode] *
rosimage.width)
rosimage.data = img.tobytes()
return rosimage
def cv2_to_imgmsg(cv2img, encoding='bgr8'):
"""
Converts an OpenCV image to a ROS image without using the cv_bridge package,
for compatibility purposes.
"""
msg = Image()
msg.width = cv2img.shape[0]
msg.height = cv2img.shape[1]
msg.encoding = encoding
msg.step = BPP[encoding] * cv2img.shape[0]
msg.data = numpy.ascontiguousarray(cv2img).tobytes()
return msg
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 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 color2graymsg(img):
img = img.astype(numpy.float32)
im = (0.299 * img[:, :, 0] + 0.587 * img[:, :, 1] +
0.114 * img[:, :, 2]).astype(numpy.uint8)
msg = Image()
msg.height = im.shape[0]
msg.width = im.shape[1]
msg.encoding = '8UC1'
msg.data = im.tostring()
msg.step = len(msg.data) // msg.height
if im.dtype.byteorder == '>':
msg.is_bigendian = True
return msg
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 fictitious_pub(self):
image_np = np.array((np.random.randint(255,
size=(self.args.height, self.args.width))), dtype=np.uint8)
image_cv = cv2.imdecode(image_np, 1)
if self.args.show:
cv2.imshow('cv_img', image_np)
cv.waitKey(2)
fictitious_msg = Image()
fictitious_msg.header.stamp = rospy.Time.now()
fictitious_msg.data = np.array(cv2.imencode('.jpg', image_cv)).tostring()
self.image_pub.publish(fictitious_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 cv_to_imgmsg(cvim, encoding = "passthrough"):
try:
return bridge.cv_to_imgmsg(cvim, encoding)
except:
img_msg = Image()
(img_msg.width, img_msg.height) = cv.GetSize(cvim)
if encoding == "passthrough":
img_msg.encoding = bridge.cvtype_to_name[cv.GetElemType(cvim)]
else:
img_msg.encoding = encoding
if encoding_as_cvtype(encoding) != cv.GetElemType(cvim):
raise CvBridgeError, "invalid encoding"
img_msg.data = cvim.tostring()
img_msg.step = len(img_msg.data) / img_msg.height
return img_msg
def publish_state_image(state_from_env1, current_state_image_pub):
current_state_image_msg = Image()
current_state_image_msg.encoding = "mono8"
current_state_image_msg.header.stamp = rospy.Time.now()
current_state_image_msg.height = 68
current_state_image_msg.width = 34
current_state_image_msg.step = 68
x = np.reshape(state_from_env1, (2,2312))
idx_x = np.argwhere(x[0] == np.amax(x[0]))
lx = idx_x.flatten().tolist()
x[1][lx[0]] = 255
x[1][lx[1]] = 255
x[1][lx[2]] = 255
y = x[1].tolist()
current_state_image_msg.data = y
current_state_image_pub.publish(current_state_image_msg)
def default(self, ci='unused'):
if not self.component_instance.capturing:
return # press [Space] key to enable capturing
image_local = self.data['image']
image = Image()
image.header = self.get_ros_header()
image.height = self.component_instance.image_height
image.width = self.component_instance.image_width
image.encoding = self.encoding
image.step = image.width * 4
# VideoTexture.ImageRender implements the buffer interface
image.data = bytes(image_local)
# fill this 3 parameters to get correcty image with stereo camera
Tx = 0
Ty = 0
R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
intrinsic = self.data['intrinsic_matrix']
camera_info = CameraInfo()
camera_info.header = image.header
camera_info.height = image.height
camera_info.width = image.width
camera_info.distortion_model = 'plumb_bob'
camera_info.D = [0]
camera_info.K = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2],
intrinsic[1][0], intrinsic[1][1], intrinsic[1][2],
intrinsic[2][0], intrinsic[2][1], intrinsic[2][2]]
camera_info.R = R
camera_info.P = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx,
intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty,
intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0]
if self.pub_tf:
self.publish_with_robot_transform(image)
else:
self.publish(image)
self.topic_camera_info.publish(camera_info)
def main():
if len(sys.argv) < 2:
print("Usage: {} dataset_name".format(sys.argv[0]))
exit(1)
file_name = sys.argv[1]
log_file = h5py.File('../dataset/log/{}.h5'.format(file_name))
camera_file = h5py.File('../dataset/camera/{}.h5'.format(file_name))
zipped_log = izip(
log_file['times'],
log_file['fiber_accel'],
log_file['fiber_gyro'])
with rosbag.Bag('{}.bag'.format(file_name), 'w') as bag:
bar = Bar('Camera', max=len(camera_file['X']))
for i, img_data in enumerate(camera_file['X']):
m_img = Image()
m_img.header.stamp = rospy.Time.from_sec(0.01 * i)
m_img.height = img_data.shape[1]
m_img.width = img_data.shape[2]
m_img.step = 3 * img_data.shape[2]
m_img.encoding = 'rgb8'
m_img.data = np.transpose(img_data, (1, 2, 0)).flatten().tolist()
bag.write('/camera/image_raw', m_img, m_img.header.stamp)
bar.next()
bar.finish()
bar = Bar('IMU', max=len(log_file['times']))
for time, v_accel, v_gyro in zipped_log:
m_imu = Imu()
m_imu.header.stamp = rospy.Time.from_sec(time)
[setattr(m_imu.linear_acceleration, c, v_accel[i]) for i, c in enumerate('xyz')]
[setattr(m_imu.angular_velocity, c, v_gyro[i]) for i, c in enumerate('xyz')]
bag.write('/fiber_imu', m_imu, m_imu.header.stamp)
bar.next()
bar.finish()
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 GetImageFromFile(im_path):
fp = open( im_path, "r" )
p = ImageFile.Parser()
while 1:
s = fp.read(307218) # trying to read a full file in one shot ...
if not s:
break
p.feed(s)
im = p.close() # we should now have an image object
im_stamp = os.path.basename(im_path).split(".")[0] #image timestamp is directly encoded in file name
im_stamp = float(im_stamp)/1000000.0
Stamp = rospy.rostime.Time.from_sec(im_stamp)
Img = Image()
Img.header.stamp = Stamp
Img.width = im.size[0]
Img.height = im.size[1]
Img.encoding = "mono8" #needs to be changed to rgb8 for rgb images
Img.step=Img.width #some nodes may complains ...
Img.header.frame_id = "camera"
Img_data = list(im.getdata()) #works for mono channels images (grayscale)
#Img_data = [pix for pix in im.getdata()]
# Img_data = [pix for pixdata in im.getdata() for pix in pixdata]
Img.data = Img_data
return (im_stamp, Stamp, Img)
def run(self):
img = Image()
r = rospy.Rate(self.config['frame_rate'])
while self.is_looping():
if self.pub_img_.get_num_connections() == 0:
if self.nameId:
rospy.loginfo('Unsubscribing from camera as nobody listens to the topics.')
self.camProxy.unsubscribe(self.nameId)
self.nameId = None
r.sleep()
continue
if self.nameId is None:
self.reconfigure(self.config, 0)
r.sleep()
continue
image = self.camProxy.getImageRemote(self.nameId)
if image is None:
continue
# Deal with the image
if self.config['use_ros_time']:
img.header.stamp = rospy.Time.now()
else:
img.header.stamp = rospy.Time(image[4] + image[5]*1e-6)
img.header.frame_id = self.frame_id
img.height = image[1]
img.width = image[0]
nbLayers = image[2]
if image[3] == kYUVColorSpace:
encoding = "mono8"
elif image[3] == kRGBColorSpace:
encoding = "rgb8"
elif image[3] == kBGRColorSpace:
encoding = "bgr8"
elif image[3] == kYUV422ColorSpace:
encoding = "yuv422" # this works only in ROS groovy and later
elif image[3] == kDepthColorSpace:
encoding = "mono16"
else:
rospy.logerr("Received unknown encoding: {0}".format(image[3]))
img.encoding = encoding
img.step = img.width * nbLayers
img.data = image[6]
self.pub_img_.publish(img)
# deal with the camera info
if self.config['source'] == kDepthCamera and image[3] == kDepthColorSpace:
infomsg = CameraInfo()
# yes, this is only for an XTion / Kinect but that's the only thing supported by NAO
ratio_x = float(640)/float(img.width)
ratio_y = float(480)/float(img.height)
infomsg.width = img.width
infomsg.height = img.height
# [ 525., 0., 3.1950000000000000e+02, 0., 525., 2.3950000000000000e+02, 0., 0., 1. ]
infomsg.K = [ 525, 0, 3.1950000000000000e+02,
0, 525, 2.3950000000000000e+02,
0, 0, 1 ]
infomsg.P = [ 525, 0, 3.1950000000000000e+02, 0,
0, 525, 2.3950000000000000e+02, 0,
0, 0, 1, 0 ]
for i in range(3):
infomsg.K[i] = infomsg.K[i] / ratio_x
infomsg.K[3+i] = infomsg.K[3+i] / ratio_y
infomsg.P[i] = infomsg.P[i] / ratio_x
infomsg.P[4+i] = infomsg.P[4+i] / ratio_y
infomsg.D = []
infomsg.binning_x = 0
infomsg.binning_y = 0
infomsg.distortion_model = ""
infomsg.header = img.header
self.pub_info_.publish(infomsg)
elif self.config['camera_info_url'] in self.camera_infos:
infomsg = self.camera_infos[self.config['camera_info_url']]
infomsg.header = img.header
self.pub_info_.publish(infomsg)
r.sleep()
if (self.nameId):
rospy.loginfo("unsubscribing from camera ")
self.camProxy.unsubscribe(self.nameId)
import numpy as np
import rospy
import sys
from sensor_msgs.msg import Image
if __name__ == '__main__':
rospy.init_node('image_publish')
name = sys.argv[1]
image = cv2.imread(name)
#cv2.imshow("im", image)
#cv2.waitKey(5)
hz = rospy.get_param("~rate", 1)
rate = rospy.Rate(hz)
pub = rospy.Publisher('image', Image, queue_size=1)
msg = Image()
msg.header.stamp = rospy.Time.now()
msg.encoding = 'bgr8'
msg.height = image.shape[0]
msg.width = image.shape[1]
msg.step = image.shape[1] * 3
msg.data = image.tostring()
pub.publish(msg)
while not rospy.is_shutdown():
pub.publish(msg)
rate.sleep()
def main_loop(self):
img = Image()
cimg = Image()
r = rospy.Rate(15)
while not rospy.is_shutdown():
if self.pub_img_.get_num_connections() == 0:
r.sleep()
continue
image = self.camProxy.getImageRemote(self.nameId)
if image is None:
continue
# Deal with the image
'''
#Images received from NAO have
if self.config['use_ros_time']:
img.header.stamp = rospy.Time.now()
else:
img.header.stamp = rospy.Time(image[4] + image[5]*1e-6)
'''
img.header.stamp = rospy.Time.now()
img.header.frame_id = self.frame_id
img.height = image[1]
img.width = image[0]
nbLayers = image[2]
if image[3] == kDepthColorSpace:
encoding = "mono16"
else:
rospy.logerr("Received unknown encoding: {0}".format(image[3]))
img.encoding = encoding
img.step = img.width * nbLayers
img.data = image[6]
self.pub_img_.publish(img)
# deal with the camera info
infomsg = CameraInfo()
infomsg.header = img.header
# yes, this is only for an XTion / Kinect but that's the only thing supported by NAO
ratio_x = float(640)/float(img.width)
ratio_y = float(480)/float(img.height)
infomsg.width = img.width
infomsg.height = img.height
# [ 525., 0., 3.1950000000000000e+02, 0., 525., 2.3950000000000000e+02, 0., 0., 1. ]
infomsg.K = [ 525, 0, 3.1950000000000000e+02,
0, 525, 2.3950000000000000e+02,
0, 0, 1 ]
infomsg.P = [ 525, 0, 3.1950000000000000e+02, 0,
0, 525, 2.3950000000000000e+02, 0,
0, 0, 1, 0 ]
for i in range(3):
infomsg.K[i] = infomsg.K[i] / ratio_x
infomsg.K[3+i] = infomsg.K[3+i] / ratio_y
infomsg.P[i] = infomsg.P[i] / ratio_x
infomsg.P[4+i] = infomsg.P[4+i] / ratio_y
infomsg.D = []
infomsg.binning_x = 0
infomsg.binning_y = 0
infomsg.distortion_model = ""
self.pub_info_.publish(infomsg)
#Currently we only get depth image from the 3D camera of NAO, so we make up a fake color image (a black image)
#and publish it under image_color topic.
#This should be update when the color image from 3D camera becomes available.
colorimg = np.zeros((img.height,img.width,3), np.uint8)
try:
cimg = self.bridge.cv2_to_imgmsg(colorimg, "bgr8")
cimg.header.stamp = img.header.stamp
cimg.header.frame_id = img.header.frame_id
self.pub_cimg_.publish(cimg)
except CvBridgeError, e:
print e
r.sleep()
def default(self, ci='unused'):
""" Publish the data of the Camera as a ROS Image message. """
if not self.component_instance.capturing:
return # press [Space] key to enable capturing
image_local = self.data['image']
image = Image()
image.header = self.get_ros_header()
image.header.frame_id += '/base_image'
image.height = self.component_instance.image_height
image.width = self.component_instance.image_width
image.encoding = 'rgba8'
image.step = image.width * 4
# VideoTexture.ImageRender implements the buffer interface
image.data = bytes(image_local)
# sensor_msgs/CameraInfo [ http://ros.org/wiki/rviz/DisplayTypes/Camera ]
# fill this 3 parameters to get correcty image with stereo camera
Tx = 0
Ty = 0
R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
intrinsic = self.data['intrinsic_matrix']
camera_info = CameraInfo()
camera_info.header = image.header
camera_info.height = image.height
camera_info.width = image.width
camera_info.distortion_model = 'plumb_bob'
camera_info.K = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2],
intrinsic[1][0], intrinsic[1][1], intrinsic[1][2],
intrinsic[2][0], intrinsic[2][1], intrinsic[2][2]]
camera_info.R = R
camera_info.P = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx,
intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty,
intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0]
self.publish(image)
self.topic_camera_info.publish(camera_info)
