def main():
rospy.init_node('VideoPublisher', anonymous=True)
VideoRaw_left = rospy.Publisher('/cam0/image_raw', Image, queue_size=10)
VideoRaw_right = rospy.Publisher('/cam1/image_raw', Image, queue_size=10)
cap = cv2.VideoCapture(int(sys.argv[1]))
cap.set(cv2.CAP_PROP_CONVERT_RGB, False)
while True:
ret, frame = cap.read()
left = frame[:, :, 0]
right = frame[:, :, 1]
left_msg = Image()
left_msg.header.stamp = rospy.Time.from_sec(time.time())
left_msg.data = left.flatten().tolist()
left_msg.width = left.shape[1]
left_msg.height = left.shape[0]
left_msg.step = left.strides[0]
left_msg.encoding = 'mono8'
left_msg.header.frame_id = 'image_rect'
right_msg = Image()
right_msg.header.stamp = rospy.Time.from_sec(time.time())
right_msg.data = right.flatten().tolist()
right_msg.width = right.shape[1]
right_msg.height = right.shape[0]
right_msg.step = right.strides[0]
right_msg.encoding = 'mono8'
right_msg.header.frame_id = 'image_rect'
VideoRaw_left.publish(left_msg)
VideoRaw_right.publish(right_msg)
rospy.sleep(0.1)
def cv_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.
"""
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
img_msg.data = cvim.tostring()
img_msg.step = len(img_msg.data) // img_msg.height
return img_msg
def cv2_to_imgmsg(cvim, encoding="passthrough"):
"""
Convert an OpenCV :cpp:type:`cv::Mat` type to a ROS sensor_msgs::Image message.
:param cvim: An OpenCV :cpp:type:`cv::Mat`
:param encoding: The encoding of the image data, one of the following strings:
* ``"passthrough"``
* one of the standard strings in sensor_msgs/image_encodings.h
:rtype: A sensor_msgs.msg.Image message
:raises CvBridgeError: when the ``cvim`` has a type that is incompatible with ``encoding``
If encoding is ``"passthrough"``, then the message has the same encoding as the image's OpenCV type.
Otherwise desired_encoding must be one of the standard image encodings
This function returns a sensor_msgs::Image message on success, or raises :exc:`cv_bridge.CvBridgeError` on failure.
"""
import numpy as np
if not isinstance(cvim, (np.ndarray, np.generic)):
raise TypeError('Your input type is not a numpy array')
img_msg = Image()
img_msg.height = cvim.shape[0]
img_msg.width = cvim.shape[1]
cv_type = '8UC3'
if encoding == "passthrough":
img_msg.encoding = cv_type
else:
img_msg.encoding = encoding
if cvim.dtype.byteorder == '>':
img_msg.is_bigendian = True
img_msg.data = cvim.tostring()
img_msg.step = len(img_msg.data) // img_msg.height
return img_msg
def 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')
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 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 image_raw_callback(self, msg):
cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough')
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
# Save a sample image
if not self.is_saved:
cv2.imwrite("/usr/src/app/dev_ws/src/semantic_segmentation/dolly.png", cv_image)
self.is_saved = True
# cv_image = cv2.resize(cv_image,(960,720))
h,w,_ = cv_image.shape
# Perform semantic segmentation
img_decoded, driveable_decoded = self.seg.process_img_driveable(cv_image,[h,w])
img_decoded = np.uint8(img_decoded * 255)
driveable_decoded = np.uint8(driveable_decoded * 255)
#Warp
warped = cv2.warpPerspective(driveable_decoded, self.M, self.img_size, flags=cv2.INTER_LINEAR)
# generate scan_msg
original_center = np.array([[[w/2,h]]],dtype=np.float32)
warped_center = cv2.perspectiveTransform(original_center, self.M)[0][0]
scan_distances, angle_increment = segmented2scan(warped, warped_center)
# publish topics
warped_msg = Image()
warped_msg = self.bridge.cv2_to_imgmsg(warped)
warped_msg.header.stamp = msg.header.stamp
warped_msg.encoding = msg.encoding
seg_img = Image()
img_msg = self.bridge.cv2_to_imgmsg(img_decoded)
seg_img.header.stamp = msg.header.stamp
seg_img.encoding = msg.encoding
scan_msg = LaserScan()
scan_msg.ranges = scan_distances
scan_msg.intensities = [0.0]*len(scan_distances)
scan_msg.header.stamp = msg.header.stamp
scan_msg.header.frame_id = 'chassis'
scan_msg.angle_increment = angle_increment
scan_msg.angle_max = 50*math.pi/180.0
scan_msg.angle_min = -50*math.pi/180.0
scan_msg.range_min = 1.0
scan_msg.range_max = 20.0
self.seg_publisher_.publish(img_msg)
self.warp_publisher_.publish(warped_msg)
if(len(scan_distances) > 50):
self.scan_publisher_.publish(scan_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 serialize(self, data):
msg = Image()
msg.header.stamp.sec = data.tm.sec
msg.header.stamp.nanosec = data.tm.nsec
msg.height = data.height
msg.width = data.width
if not data.format:
msg.encoding = "rgb8"
else:
msg.encoding = data.format
msg.step = 1920
msg.data = data.pixels
return OpenRTM_aist.ByteDataStreamBase.SERIALIZE_OK, 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 airpub():
pub_left = rospy.Publisher("/gi/simulation/left/image_raw", Image, queue_size=1)
pub_right=rospy.Publisher("/gi/simulation/right/image_raw", Image, queue_size=1)
rospy.init_node('airsim_info', anonymous=True)
rate = rospy.Rate(10) # 10hz
# connect to the AirSim simulator
client = airsim.MultirotorClient()
client.confirmConnection()
while not rospy.is_shutdown():
# get camera images from the car
Imu = client.getImuData()
responses = client.simGetImages([
airsim.ImageRequest("front-left", airsim.ImageType.Scene, False, False),
airsim.ImageRequest("front-right", airsim.ImageType.Scene, False, False)
])
img_dic={}
for idx, response in enumerate(responses):
img_rgb_string = response.image_data_uint8
img_dic[idx]=img_rgb_string
# Populate image message
msg_left=Image()
msg_left.header.stamp = rospy.Time.now()
msg_left.header.frame_id = "frameId"
msg_left.encoding = "rgb8"
msg_left.height = 480 # resolution should match values in settings.json
msg_left.width = 752
msg_left.data = img_dic[0]
msg_left.is_bigendian = 0
msg_left.step = msg_left.width * 3
msg_right = Image()
msg_right.header.stamp = rospy.Time.now()
msg_right.header.frame_id = "frameId"
msg_right.encoding = "rgb8"
msg_right.height = 480 # resolution should match values in settings.json
msg_right.width = 752
msg_right.data = img_dic[1]
msg_right.is_bigendian = 0
msg_right.step = msg_left.width * 3
# log time and size of published image
rospy.loginfo(len(response.image_data_uint8))
# publish image message
pub_left.publish(msg_left)
pub_right.publish(msg_right)
# sleep until next cycle
rate.sleep()
def 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 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 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 __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 createImage(h, w):
ret = Image()
ret.height = h
ret.width = w
ret.encoding = 'rgb8'
ret.step = w * 3
return ret
def publishImage(self, type="color"):
responses = self.client.simGetImages([
airsim.ImageRequest("1", airsim.ImageType.Scene, False, False)
]) #scene vision image in uncompressed RGB array
for response in responses:
img_rgb_string = response.image_data_uint8
# Populate image message
msg = Image()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "frameId"
msg.encoding = "rgb8"
msg.height = 360 # resolution should match values in settings.json
msg.width = 640
msg.data = img_rgb_string
msg.is_bigendian = 0
msg.step = msg.width * 4
# log time and size of published image
rospy.loginfo(len(response.image_data_uint8))
# publish image message
# msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()
self.image_pub.publish(msg)
# sleep until next cycle
rospy.Rate(10).sleep()
def run(self):
self.resume()
while True:
with self.state:
if self.paused:
self.state.wait()
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")
resized = cv2.resize(cv_image, (125, 125),
interpolation=cv2.INTER_AREA)
#cv2.imshow("Image window", resized)
cv2.imwrite(
self.image_directory + '/shot_' + str(self.counter) + '.png',
resized)
self.counter += 1
cv2.waitKey(1)
def publishing(pub_image, pub_camera, image, type_of_camera):
if type_of_camera is 1:
image.convert(carla.ColorConverter.Depth)
elif type_of_camera is 2:
image.convert(carla.ColorConverter.CityScapesPalette)
array = np.frombuffer(image.raw_data, dtype=np.uint8)
array = np.reshape(array, (image.height, image.width, 4))
array = array[:, :, :3]
array = array[:, :, ::-1]
img = Image()
infomsg = CameraInfo()
img.header.stamp = rospy.Time.now()
img.header.frame_id = 'base'
img.height = infomsg.height = image.height
img.width = infomsg.width = image.width
img.encoding = "rgb8"
img.step = img.width * 3 * 1
st1 = array.tostring()
img.data = st1
cx = infomsg.width / 2.0
cy = infomsg.height / 2.0
fx = fy = infomsg.width / (2.0 * math.tan(image.fov * math.pi / 360.0))
infomsg.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1]
infomsg.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1, 0]
infomsg.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0]
infomsg.D = [0, 0, 0, 0, 0]
infomsg.binning_x = 0
infomsg.binning_y = 0
infomsg.distortion_model = "plumb_bob"
infomsg.header = img.header
pub_image.publish(img)
pub_camera.publish(infomsg)
def timer_callback(self):
ret, frame = self.cap.read()
msg = Image()
header = Header()
header.frame_id = str(self.counter)
header.stamp = self.get_clock().now().to_msg()
if not ret:
self.get_logger().warning('Publishing RANDOM IMAGE "%s"' %
str(header.stamp))
frame = np.random.randint(255, size=(480, 640, 3), dtype=np.uint8)
if self.stream_video:
cv2.imshow('frame', frame)
cv2.waitKey(1)
frame = cv2.resize(frame,
dsize=(64, 64),
interpolation=cv2.INTER_CUBIC)
frame = np.flipud(frame)
frame = np.fliplr(frame)
msg.header = header
msg.height = frame.shape[0]
msg.width = frame.shape[1]
msg.encoding = "bgr8"
value = self.bridge.cv2_to_imgmsg(frame.astype(np.uint8))
msg.data = value.data
self.publisher_.publish(msg)
self.get_logger().info('Publishing Image "%s"' % str(header.stamp))
self.counter += 1
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 image_msg(
pixels: np.ndarray,
stamp: rospy.rostime.Time,
dims: tuple,
encoding: str,
) -> Image:
"""
Return a ROS image from an RGB NumPy tensor.
Args:
pixels: the 3D NumPy tensor to convert to an ROS Image
stamp: the timestamp to use for the image message
dims: a tuple of the height and width of the image
encoding: the encoding to use for the image (e.g., 'rgb8')
Returns:
an image with the raw pixel data
"""
ros_image = Image()
ros_image.header.stamp = stamp
ros_image.header.frame_id = "camera"
ros_image.height = dims[0]
ros_image.width = dims[1]
ros_image.encoding = encoding
ros_image.data = pixels.flatten().tobytes()
return ros_image
def timer_callback(self):
"""Timer Callback Function
This method captures images and publishes required data in ros 2 topic.
"""
if self.capture.isOpened():
# reads image data
ret, frame = self.capture.read()
# processes image data and converts to ros 2 message
msg = Image()
msg.header.stamp = Node.get_clock(self).now().to_msg()
msg.header.frame_id = 'ANI717'
msg.height = np.shape(frame)[0]
msg.width = np.shape(frame)[1]
msg.encoding = "bgr8"
msg.is_bigendian = False
msg.step = np.shape(frame)[2] * np.shape(frame)[1]
msg.data = np.array(frame).tobytes()
# publishes message
self.publisher_.publish(msg)
self.get_logger().info('%d Images Published' % self.i)
# image counter increment
self.i += 1
return None
def 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 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 detect(self):
# call detectron2 model
outputs = self.predictor(self.img)
# process pointcloud to get 3D position and size
detections = self.process_points(outputs)
# publish detection result
obstacle_array = ObstacleArray()
obstacle_array.header = self.header
if self.detect_obj_pub.get_subscription_count() > 0:
obstacle_array.obstacles = detections
self.detect_obj_pub.publish(obstacle_array)
# visualize detection with detectron API
if self.detect_img_pub.get_subscription_count() > 0:
v = Visualizer(self.img[:, :, ::-1],
MetadataCatalog.get(self.cfg.DATASETS.TRAIN[0]),
scale=1)
out = v.draw_instance_predictions(outputs["instances"].to("cpu"))
out_img = out.get_image()[:, :, ::-1]
out_img_msg = Image()
out_img_msg.header = self.header
out_img_msg.height = out_img.shape[0]
out_img_msg.width = out_img.shape[1]
out_img_msg.encoding = 'rgb8'
out_img_msg.step = 3 * out_img.shape[1]
out_img_msg.data = out_img.flatten().tolist()
self.detect_img_pub.publish(out_img_msg)
def main():
rospy.init_node('vision_pub_img')
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('/camera/rgb/image_raw', 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 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 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 camera_node():
cap = cv2.VideoCapture(Configs.video_path)
fps = cap.get(cv2.CAP_PROP_FPS)
rospy.init_node('camera', anonymous=True)
pub = rospy.Publisher('/camera_image', Image, queue_size=10)
rate = rospy.Rate(Configs.camera_rate) # how many in one minute
i = 0
while not rospy.is_shutdown():
try:
ret, frame = cap.read()
i += 1
if not ret and Configs.video_start:
cap = cv2.VideoCapture(Configs.video_path)
print("*************** Restart ****************")
i = 0
time.sleep(2)
continue
image_temp = Image()
header = Header(stamp=rospy.Time.now())
header.frame_id = 'camera'
image_temp.height = frame.shape[0]
image_temp.width = frame.shape[1]
image_temp.encoding = 'bgr8'
image_temp.data = frame.tostring()
image_temp.header = header
image_temp.step = frame.shape[1] * 3
pub.publish(image_temp)
rate.sleep()
print("Frame " + str(i) + ", Time " + str(int(i / fps)) +
" s, Image: " + str(frame.shape[0]) + "*" + str(frame.shape[1]))
except Exception as ex:
print(ex)
def camera_setting(index, image, camera, lidar_obj, f, camera_name, bag, rate):
camera_image = Image()
camera_image.header.frame_id = "/base_link"
camera_image.header.stamp.secs = int(camera.timestamps[index])
camera_image.header.stamp.nsecs = int((camera.timestamps[index] - camera_image.header.stamp.secs) * (10**9))
camera_image.height = image.height
camera_image.width = image.width
camera_image.encoding = "rgb8"
#original image
##########
camera_image.data = np.array(image).tostring()
##########
#distances projection image
###########
#camera_image.data = np.array(distances_projection(image, lidar_obj.data[index], camera.poses[index], camera.intrinsics)).tostring()
##########
#semantic segmentation projection image
##########
#camera_image.data = np.array(semantic_segmentation_projection(image, lidar_obj.data[index], camera.poses[index], camera.intrinsics, f.semseg[index])).tostring()
#tmp_img = semantic_segmentation_projection(image, lidar_obj.data[index], camera.poses[index], camera.intrinsics, f.semseg[index])
#camera_image.data = np.array(tmp_img).tostring()
##########
topic_name = "/camera_image/" + camera_name
bag.write(topic_name, camera_image, camera_image.header.stamp)
rate.sleep()
def applyDepthMask(self, image_msg, mask_msg, blur):
# height and width
height = image_msg.height
width = image_msg.width
# get images
image_raw = np.fromstring(image_msg.data, np.uint8)
image_blur = np.full((height * width * 3, 1), 225)
mask = np.fromstring(mask_msg.data, np.uint8)
# reshape arrrays (fromstring) to matrices
mask = mask.reshape(height * width, 1)
image_raw = image_raw.reshape(height * width * 3, 1)
# for index i in images,
# select image_raw[i] when mask[i] true
# select image_blur[i] when mask[i] false
image_masked = np.empty(image_raw.shape)
for i in range(3):
image_masked[i::3] = np.where(mask, image_raw[i::3], image_blur[i::3])
# create sensor_msgs image
image_msg_out = Image()
image_msg_out.header.stamp = image_msg.header.stamp
image_msg_out.header.frame_id = image_msg.header.frame_id
image_msg_out.height = image_msg.height
image_msg_out.width = image_msg.width
image_msg_out.encoding = image_msg.encoding
image_msg_out.is_bigendian = image_msg.is_bigendian
image_msg_out.step = image_msg.step
image_msg_out.data = list(image_masked)
# result
return image_msg_out
def timer_callback(self):
global logmul
global maxsize
global datacount
if self._datasize > maxsize:
print("finish")
self.destroy_node()
return
msg = Image()
if self._datasize < 300:
msg.height = 1
else:
msg.height = 100
msg.width = int(self._datasize/msg.height/3)
msg.encoding = "rgb8"
msg.step = 1920
msg.data = b" "*(msg.height*msg.width*3)
tm = time.time()
tm_f = float(tm - int(tm))
msg.header.stamp.sec = int(float(tm) - float(tm_f))
msg.header.stamp.nanosec = int(float(tm_f) * float(1000000000))
self.publisher_.publish(msg)
#self.get_logger().info('Publishing: "%s"' % msg.data)
self._count += 1
if self._count % datacount == 0:
print(msg.height*msg.width*3)
self._datasize = int(
self._datasize*logmul[int(self._count/datacount) % 3])
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], "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]
# Img.width = 1280
# Img.height = 720
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 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 step(self):
stamp = super().step()
if not stamp:
return
# Publish camera data
if self._image_publisher.get_subscription_count(
) > 0 or self._always_publish:
self._wb_device.enable(self._timestep)
# Image data
msg = Image()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
msg.height = self._wb_device.getHeight()
msg.width = self._wb_device.getWidth()
msg.is_bigendian = False
msg.step = self._wb_device.getWidth() * 4
if self._wb_device.getRangeImage() is None:
return
image_array = np.array(self._wb_device.getRangeImage(),
dtype="float32")
# We pass `data` directly to we avoid using `data` setter.
# Otherwise ROS2 converts data to `array.array` which slows down the simulation as it copies memory internally.
# Both, `bytearray` and `array.array`, implement Python buffer protocol, so we should not see unpredictable
# behavior.
# deepcode ignore W0212: Avoid conversion from `bytearray` to `array.array`.
msg._data = image_array.tobytes()
msg.encoding = '32FC1'
self._image_publisher.publish(msg)
else:
self._wb_device.disable()
def _publish_image(self):
"""
Publish latest camera image as Image with CameraInfo.
"""
# only publish if we have a subscriber
if self._image_pub.get_num_connections() == 0:
return
# get latest image from cozmo's camera
camera_image = self._cozmo.world.latest_image
if camera_image is not None:
# convert image to gray scale as it is gray although
img = camera_image.raw_image.convert('L')
ros_img = Image()
ros_img.encoding = 'mono8'
ros_img.width = img.size[0]
ros_img.height = img.size[1]
ros_img.step = 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)
# publish images and camera info
self._image_pub.publish(ros_img)
camera_info = self._camera_info_manager.getCameraInfo()
camera_info.header = ros_img.header
self._camera_info_pub.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()
while not rospy.is_shutdown():
# get camera images from the car
responses = client.simGetImages([
airsim.ImageRequest("1", airsim.ImageType.Scene, False, False)
]) #scene vision image in uncompressed RGB array
for response in responses:
img_rgb_string = response.image_data_uint8
# Populate image message
msg = Image()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "frameId"
msg.encoding = "rgb8"
msg.height = 360 # resolution should match values in settings.json
msg.width = 640
msg.data = img_rgb_string
msg.is_bigendian = 0
msg.step = msg.width * 3
# log time and size of published image
rospy.loginfo(len(response.image_data_uint8))
# publish image message
pub.publish(msg)
# sleep until next cycle
rate.sleep()
def vis_callback(data):
img = np.frombuffer(data.data, dtype=np.uint8).reshape(data.height, data.width, 4)
#fixing bgr
img = img[:,:,0:3]
img = img[...,::-1]
results, outimg = inference(img)
outmsg = ROS_Image()
outmsg.data = outimg.tostring()
outmsg.encoding = "bgr8"
outmsg.header.stamp = rospy.Time.now()
# outimg = PIL_Image.fromarray(out, 'RGB')
# outimg.show()
image_pub.publish(outmsg)
results_pub.publish(jsonpickle.encode(results))
#show or save here
#oimg = PIL_Image.fromarray(outimg, 'RGB')
#oimg.show()
#oimg.save("/home/tomek/mercedes-clk-perception/images/loop2_precise/{}.jpg".format(int(time.time())))
pub_rate.sleep()
def _create_frame_message(self, frame):
msg = Image()
msg.height = FRAME_HEIGHT
msg.width = FRAME_WIDTH
msg.encoding = 'rgb8'
msg.data = np.ravel(frame).tolist()
return msg
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 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 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 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 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 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 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 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 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)
