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 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()
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 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 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 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 __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 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 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 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 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 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 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 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 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 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 depth_to_imgmsg(deptharray):
"""
Converts an depth or disp image to a ROS image without using the cv_bridge package,
for compatibility purposes.
Depth array has the shape of [H, W, 1]
"""
msg = Image()
msg.height = deptharray.shape[0]
msg.width = deptharray.shape[1]
msg.encoding = '32FC1'
if deptharray.dtype.byteorder == '>':
msg.is_bigendian = True
msg.data = deptharray.tostring()
msg.step = len(msg.data) // msg.height
return msg
def talker():
pub = rospy.Publisher('chatter', String, queue_size=10)
pub2 = rospy.Publisher('test_image',Image, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(0.2) # 10hz
while not rospy.is_shutdown():
hello_str = "hello world %s" % rospy.get_time()
#rospy.loginfo(hello_str)
pub.publish(hello_str)
test_image = Image()
test_image.header.stamp = rospy.Time.now()
test_image.height = 3
test_image.width = 3
test_image.data = [0,255,0,0,255,0,0,255,0]
pub2.publish(test_image)
rate.sleep()
def main():
global raw_image_msg
rospy.init_node("object_tracing")
rospy.Subscriber("/raw_image", Image, object_tracing_callback)
image_pub = rospy.Publisher("/image", Image, queue_size=10)
while raw_image_msg is None:
continue
image_array = list_to_array(raw_image_msg.data, raw_image_msg.height,
raw_image_msg.width)
while np.all(image_array == 0):
print("zeros!")
image_array = list_to_array(raw_image_msg.data, raw_image_msg.height,
raw_image_msg.width)
continue
tracker = Tracker(image_array)
while not rospy.is_shutdown():
image_array = list_to_array(raw_image_msg.data, raw_image_msg.height,
raw_image_msg.width)
# cv2.imwrite("./input.png", image_array)
# print(image_array)
# transform image_array to output
pts, output = tracker.track(image_array, method="meanshift")
# pts, output = tracker.track(image_array, method="camshift")
print(pts)
# print(np.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 on_frame(self, frame):
if self.pub is not None:
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.seq = self.seq_num
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = "0"
self.pub.publish(msg)
self.seq_num += 1
def cv2_to_imgmsg(self, 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 cv2
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]
if len(cvim.shape) < 3:
cv_type = self.dtype_with_channels_to_cvtype2(cvim.dtype, 1)
else:
cv_type = self.dtype_with_channels_to_cvtype2(
cvim.dtype, cvim.shape[2])
if encoding == "passthrough":
img_msg.encoding = cv_type
else:
img_msg.encoding = encoding
# Verify that the supplied encoding is compatible with the type of the OpenCV image
if self.cvtype_to_name[self.encoding_to_cvtype2(
encoding)] != cv_type:
raise CvBridgeError(
"encoding specified as %s, but image has incompatible type %s"
% (encoding, cv_type))
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 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 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()
rospy.loginfo("Connection confirmed")
client.enableApiControl(True, "Drone1")
client.enableApiControl(True, "Drone2")
state1 = client.getMultirotorState(vehicle_name="Drone1")
s = pprint.pformat(state1)
rospy.loginfo("%s", s)
while not rospy.is_shutdown():
# get camera images from the car
rospy.loginfo("Retrieving image")
responses = client.simGetImages(
[airsim.ImageRequest("1", airsim.ImageType.Scene, False, False)],
vehicle_name="Drone1"
) #scene vision image in uncompressed RGBA array
for response in responses:
img_rgba_string = response.image_data_uint8
rospy.loginfo("Populating image")
# 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)
rospy.loginfo("Published image")
# sleep until next cycle
rate.sleep()
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 load_image_to_ros_msg(filename):
image_np = cv2.imread(filename, cv2.IMREAD_GRAYSCALE)
timestamp_nsecs = os.path.splitext(os.path.basename(filename))[0]
timestamp = rospy.Time(secs=int(timestamp_nsecs[0:-9]),
nsecs=int(timestamp_nsecs[-9:]))
rosimage = Image()
rosimage.header.stamp = timestamp
rosimage.height = image_np.shape[0]
rosimage.width = image_np.shape[1]
# only with mono8! (step = width * byteperpixel * numChannels)
rosimage.step = rosimage.width
rosimage.encoding = "mono8"
rosimage.data = image_np.tostring()
return rosimage, timestamp
def publish_output(self, np_out, header):
property_out = Image(encoding="32FC1")
property_out.header = header
property_out.height = self.shape[0]
property_out.width = self.shape[1]
if self.args.likelihood_loss:
contig = np.ascontiguousarray(np_out[0])
contig_std = np.ascontiguousarray(np_out[1])
else:
contig = np.ascontiguousarray(np_out)
property_out.step = contig.strides[0]
if self.args.likelihood_loss:
std_out = property_out
std_out.data = contig_std.tostring()
self.std_pub.publish(std_out)
property_out.data = contig.tostring()
# Publish
self.property_pub.publish(property_out)
def target_map_sub(target_map):
global pub
ndata_f = target_map.data[1].data
ndata_f.resize(300,300)
ndata = ndata_f * 65535;
ndata = ndata.astype('int16')
im = Image(encoding='mono16')
im.height, im.width, channels = ndata.shape + (1,)
contig = np.ascontiguousarray(ndata)
im.data = contig.tostring()
im.step = contig.strides[0]
im.is_bigendian = (
ndata.dtype.byteorder == '>' or
ndata.dtype.byteorder == '=' and sys.byteorder == 'big'
)
pub.publish(im)
def publish_inferenced_img(self, img: np.ndarray, boxes: np.ndarray,
cone_colors: np.ndarray):
for box, cone_color in zip(boxes, cone_colors):
x, y, w, h = box.astype(int)
cv2.rectangle(img, (x, y), (x + w, y + h), self.colors[cone_color],
2)
img_msg = Image()
img_msg.header.stamp = rp.Time.now()
img_msg.header.frame_id = "/fsds_utils/camera/inferenced_image"
img_msg.height = img.shape[0]
img_msg.width = img.shape[1]
img_msg.encoding = "bgr8"
img_msg.is_bigendian = 0
img_msg.data = img.flatten().tostring()
img_msg.step = len(img_msg.data) // img_msg.height
self.inferenced_img_pub.publish(img_msg)
def _publish_combined_global_poses(self, data: np.ndarray) -> None:
resolution = (400, 400)
pos0, w0, h0, pos1, w1, h1 = calculate_bounding_box(
state=data, orientation=(0, 0, 1), resolution=resolution)
frame = np.zeros(resolution)
frame[pos0[1] - w0 // 2:pos0[1] + w0 // 2,
pos0[0] - h0 // 2:pos0[0] + h0 // 2] = 255
try:
frame[pos1[1] - w1 // 2:pos1[1] + w1 // 2,
pos1[0] - h1 // 2:pos1[0] + h1 // 2] = 125
except TypeError:
pass
image = Image()
image.data = frame.astype(np.uint8).flatten().tolist()
image.height = resolution[0]
image.width = resolution[1]
image.encoding = 'mono8'
self._publisher.publish(image)
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 publish_image(self, imgdata, height, width, time=None):
image_raw=Image_msg()
header = Header(stamp=rospy.Time.now())
header.frame_id = 'map'
image_raw.height = height
image_raw.width = width
image_raw.encoding='rgb8'
image_raw.data=np.array(imgdata).tostring()
image_raw.header=header
image_raw.step=1241*3
self.image_pubulisher_raw.publish(image_raw)
image_compressed = CompressedImage()
image_compressed.header.stamp =rospy.Time.now()
image_compressed.format = "jpeg"
imgdata = cv2.cvtColor(imgdata, cv2.COLOR_BGR2RGB)
image_compressed.data = np.array(cv2.imencode('.jpg', imgdata)[1]).tostring()
self.image_pubulisher_compressed.publish(image_compressed)
def publish_image():
if drone.is_new_frame_ready():
frame = drone.get_last_frame()
#gray=cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)
#edged = cv2.Canny(gray, 30, 150)
#cv2.imshow("image",frame)
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(frame).tostring()
#print(imgdata)
#image_temp.is_bigendian=True
image_temp.header=header
image_temp.step=960*3
time.sleep(0.1)
image_pubulish.publish(image_temp)
def convert_pil_to_ros_img(self, img):
"""Function for converting pillow to ros image
Args:
img (PIL.Image.Image): Pillow image that represents GUI
Returns:
sensor_msgs.msg._Image.Image: ROS image for image publisher
"""
img = img.convert('RGB')
msg = ROSImage()
stamp = rospy.Time.now()
msg.height = img.height
msg.width = img.width
msg.encoding = "rgb8"
msg.is_bigendian = False
msg.step = 3 * img.width
msg.data = np.array(img).tobytes()
return msg
def update_power_meter(data, publisher):
"""Update the power meter with battery reading."""
battery_percent = float(data.percentage)
image = Image.new("1", (OLED_WIDTH, OLED_HEIGHT))
draw = ImageDraw.Draw(image)
draw.rectangle([(0, OLED_HEIGHT // 2),
(int(OLED_WIDTH * battery_percent), OLED_HEIGHT)],
fill=0xff)
percent_text = str(int(battery_percent * 100))
status_text = "Unknown"
if data.power_supply_status == BatteryState.POWER_SUPPLY_STATUS_FULL:
status_text = "Charged"
elif data.power_supply_status == BatteryState.POWER_SUPPLY_STATUS_CHARGING:
status_text = "Charging"
percent_text = "??"
elif data.power_supply_status == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING:
status_text = "Discharging"
elif data.power_supply_status == BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING:
status_text = "Slow Charging"
percent_text = "??"
draw.text((FONT_PADDING, FONT_PADDING),
"Battery: " + percent_text + "%",
font=FONT,
fill=0xff)
draw.text((FONT_PADDING, OLED_HEIGHT - 2 * FONT_PADDING - FONT_SIZE),
status_text,
font=FONT,
fill=0x00)
image_message = ImageMessage()
image_message.data = image.convert("L").tobytes()
image_message.width = image.width
image_message.height = image.height
image_message.step = image.width
image_message.encoding = "mono8"
publisher.publish(image_message)
def CreateMonoBag(imgs,bagname,yamlName):
'''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], "rb" )
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]
if im.mode=='RGB': #(3x8-bit pixels, true color)
Img.encoding = "rgb8"
Img.header.frame_id = "camera_rgb_optical_frame"
Img.step = Img.width*3
Img_data = [pix for pixdata in im.getdata() for pix in pixdata]
elif im.mode=='L': #(8-bit pixels, black and white)
Img.encoding = "mono8"
Img.header.frame_id = "camera_gray_optical_frame"
Img.step = Img.width
Img_data=[pix for pixdata in [im.getdata()] for pix in pixdata]
Img.data = Img_data
[calib, cameraName]=yaml_to_CameraInfo(yamlName)
calib.header.stamp = Stamp
if im.mode=='RGB':
calib.header.frame_id = "camera_rgb_optical_frame"
elif im.mode=='L':
calib.header.frame_id = "camera_gray_optical_frame"
bag.write( cameraName + '/camera_info', calib, Stamp)
bag.write( cameraName + '/image_raw', Img, Stamp)
finally:
bag.close()
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()
'''read image size'''
imgpil = ImagePIL.open(imgs[0])
width, height = imgpil.size
# print "size:",width,height
# width 1241, height 376
while 1:
s = fp.read(1024)
if not s:
break
p.feed(s)
im = p.close()
Stamp = rospy.rostime.Time.from_sec(time.time())
'''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)
finally:
bag.close()
def flow_to_imgmsg(flowarray):
"""
Converts an flow image to a ROS image without using the cv_bridge package,
for compatibility purposes.
flow array has the shape of [H, W, 2]
"""
msg = Image()
#print(flowarray.shape)
msg.height = flowarray.shape[0]
msg.width = flowarray.shape[1]
msg.encoding = '32FC2'
if flowarray.dtype.byteorder == '>':
msg.is_bigendian = True
msg.data = flowarray.tostring()
#print(len(msg.data))
msg.step = len(msg.data) // msg.height
return msg
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('client')
cli = node.create_client(ImageDetection, 'image_detection')
while not cli.wait_for_service(timeout_sec=1.0):
print('service not available, waiting again...')
#br = CvBridge()
#dtype, n_channels = br.encoding_as_cvtype2('8UC3')
IMG_PATH = "/root/ros2-tensorflow/data/dogs.jpg"
img = cv2.imread(IMG_PATH, cv2.IMREAD_COLOR)
#img_msg = br.cv2_to_imgmsg(img)
img_msg = Image()
img_msg.height = img.shape[0]
img_msg.width = img.shape[1]
img_msg.encoding = "rgb8"
img_msg.data = img.tostring()
img_msg.step = len(img_msg.data) // img_msg.height
img_msg.header.frame_id = "world"
req = ImageDetection.Request()
req.image = img_msg
future = cli.call_async(req)
rclpy.spin_until_future_complete(node, future)
if future.result() is not None:
node.get_logger().info('Result of classification: %r' % future.result().detections)
else:
node.get_logger().error('Exception while calling service: %r' % future.exception())
node.destroy_node()
rclpy.shutdown()
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 CreateBag(args):
imgs = GetImages(args[0])
if not imgs:
print("No images found in %s" % dir)
exit()
rosbagfile = args[1]
if (os.path.exists(rosbagfile)):
os.remove(rosbagfile)
bag = rosbag.Bag(rosbagfile, 'w')
try:
for filename in imgs:
print("Adding %s" % filename)
with open(filename, "rb") as image_data:
parser = ImageFile.Parser()
while 1:
raw_bytes = image_data.read(1024)
if not raw_bytes:
break
parser.feed(raw_bytes)
parsed_image = parser.close()
timestamp = rospy.Time.from_sec(time.time())
image = Image()
image.header.stamp = timestamp
image.width = parsed_image.size[0]
image.height = parsed_image.size[1]
image.step = image.width * 4
image.encoding = "rgba8"
image.header.frame_id = "image_data/image"
image.data = [
pix for pixdata in parsed_image.getdata()
for pix in pixdata
]
bag.write('image_node/image_raw', image, timestamp)
finally:
bag.close()
def nparray_to_rosimg(self, array):
""" @brief converts numpy 2D array to ROS image
@param[in] 2D numpy array of integers
"""
# Get min-max range
array[array > 1E308] = 0
array[array < 0] = 0
min = np.min(array)
max = np.max(array)
array = ((array - min) / (max - min)) * 255
# Scale values between 0 and 255
image = Image()
image.encoding = "mono8"
image.is_bigendian = 0
image.width = array.shape[0]
image.height = array.shape[1]
image.step = array.shape[0]
image.data = [int(i) for i in array.flatten('F')]
return image
def _set_goal_state(self, array, bounds, color='cyan'):
self.goal_array = array
marker = self._make_state_marker(array, bounds, color=color)
data, ev = self.network_handler.evaluate_array(self.goal_array)
data = data.copy()
data = data[:, ::-1, :]
data = cv2.resize(data, (2 * 480, 480))
bits = data.tobytes()
goal_img_msg = Image(
header=Header(0, rospy.Time.now(), 'table_center'))
goal_img_msg.width = 2 * 480
goal_img_msg.height = 480
goal_img_msg.step = 3 * 2 * 480
goal_img_msg.data = bits
goal_img_msg.encoding = 'rgb8'
self.mdn_goal_pub.publish(goal_img_msg)
self.state_marker_pub.publish(marker)
self.goal_img_msg = goal_img_msg
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 run(data):
frame = np.frombuffer(data.data,
dtype=np.uint8).reshape(data.height, data.width, -1)
tstart = time.time()
height = frame.shape[0]
width = frame.shape[1]
frame1 = cv.resize(frame, (576, 324))
frame_np = frame1[:, :, [2, 1, 0]]
# test
img = Image()
img.encoding = "bgr8"
img.height = height
img.width = width
# img.step = (width) * sizeof(float)
img.step = img.width * 8 * 3
img.is_bigendian = 0
img.data = np.asarray(frame, np.uint8).tostring()
raw_video_pub.publish(img)
print("in")
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 ros_sensor_image_from_matrix(self, mat, normalize=False):
image_message = RosImage()
image_message.header.stamp = rospy.Time.now()
height = mat.ravel().astype(np.uint8).tolist()
image_message.data = []
if normalize:
normalizer = matplotlib.colors.Normalize()
normalizer.autoscale(height)
for el in height:
if normalize:
image_message.data += np.array([
(255 * (1 - normalizer(el))), 0, (255 * normalizer(el))
]).astype(np.uint8).tolist()
else:
image_message.data += [255 - el, 0, el]
image_message.height = len(mat)
image_message.width = len(mat[0])
image_message.is_bigendian = 0
image_message.encoding = 'bgr8'
return image_message
def get_snapshot():
for i in range(0, 10):
s = rospy.ServiceProxy("return_camera_image", ReturnImages)
img_data = Image()
img_data.width = s([]).width
img_data.height = s([]).height
img_data.encoding = s([]).encoding
img_data.data = s([]).data
bridge = CvBridge()
cv_image = bridge.imgmsg_to_cv2(img_data, "passthrough")
cv_resized = cv2.resize(cv_image, (125, 125),
interpolation=cv2.INTER_AREA)
#cv2.imshow("Image window", cv_image)
cv2.waitKey(1)
return cv_resized
def _receive_message(self,message):
global my
rospy.loginfo(rospy.get_caller_id() + " I heard a message of %s",self._message_type)
rospy.loginfo(rospy.get_caller_id() + " Time from previous message %s",(rospy.get_time()-my) )#edw mou leei unicode type
my=rospy.get_time()
try:
msg=Image()
msg.header.seq=message['header']['seq']
msg.header.stamp.secs=message['header']['stamp']['secs']
msg.header.stamp.nsecs=message['header']['stamp']['nsecs']
msg.header.frame_id=message['header']['frame_id']
msg.height=message['height']
msg.width=message['width']
msg.encoding=str(message['encoding'])
msg.is_bigendian=message['is_bigendian']
msg.step=message['step']
msg.data=message['data'].decode('base64')
self._rosPub=rospy.Publisher(self._local_topic_name, Image, queue_size=10) #message type is String instead of msg_stds/String
self._rosPub.publish(msg)
except:
print('Error')
def main(args):
FPS = 60
B = 0
dir = 1
# Init ROS node
rospy.init_node('image_publisher', anonymous=True)
pub = rospy.Publisher("/ros/color/image_raw", Image, queue_size=1)
rate = rospy.Rate(FPS)
print("Begin Publishing")
count = 0
ts_start = time.perf_counter()
IMG_WIDTH = 640
IMG_HEIGHT = 480
while not rospy.is_shutdown():
image_np = np.zeros((IMG_HEIGHT, IMG_WIDTH, 3), dtype=np.uint8)
B += 1 * dir
if B == 255:
dir = -1
elif B == 0:
dir = 1
image_np[:, :, 0] = B
# Create Image
image_np = cv2.cvtColor(image_np, cv2.COLOR_BGR2RGB)
msg = Image()
msg.header.stamp = rospy.Time.now()
msg.height = image_np.shape[0]
msg.width = image_np.shape[1]
msg.encoding = "rgb8"
# "bgr8" is not supported by Isaac SDK
msg.data = image_np.tostring()
# Publish new image
pub.publish(msg)
count += 1
delta = time.perf_counter() - ts_start
# Log
print("Sent", count, "images in", round(delta), "seconds with",
round(count / delta, 2), "FPS")
rate.sleep()
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 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)
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 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)
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)
