def handle_pkt(pkt=None):
ts_begin_loc = pkt.find(begin_timestamp_marker)
ts_end_loc = pkt.find(end_timestamp_marker)
if ts_begin_loc == -1 or ts_end_loc == -1:
#something's fishy, discard jpeg
print "JPEG discarded, malformed data"
return
ts = float(pkt[ts_begin_loc+len(begin_timestamp_marker):ts_end_loc])
# if ts > last_ts:
# last_ts = ts
# else:
# return
pkt = pkt[ts_end_loc+len(end_timestamp_marker):]
print "{} bytes of JPEG recvd".format(len(pkt))
msg = CompressedImage()
msg.header.stamp = rospy.Time(tango_clock_offset + float(ts))
msg.header.frame_id = camera_name
msg.data = pkt
msg.format = 'jpeg'
pub_camera.publish(msg)
def callback(self, ros_data):
#### direct conversion to CV2 ####
np_arr = np.fromstring(ros_data.data, np.uint8)
image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
#(rows,cols,channels) = image_np.shape
#image_np = image_np[0:64, 0:480]
#equ = cv2.equalizeHist(image_np)
#res = np.hstack((image_np,equ))
#### Feature detectors using CV2 ####
# "","Grid","Pyramid" +
# "FAST","GFTT","HARRIS","MSER","ORB","SIFT","STAR","SURF", "GridFAST"
#method = "GridFAST"
#feat_det = cv2.FeatureDetector_create(method)
#time1 = time.time()
# convert np image to grayscale
#featPoints = feat_det.detect( cv2.cvtColor(image_np, cv2.COLOR_BGR2GRAY))
#time2 = time.time()
#if VERBOSE :
# print '%s detector found: %s points in: %s sec.'%(method, len(featPoints),time2-time1)
#for featpoint in featPoints:
# x,y = featpoint.pt
# cv2.circle(image_np,(int(x),int(y)), 3, (0,0,255), -1)
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()
# Publish new image
self.image_pub.publish(msg)
def grabAndPublish(self,stream,publisher):
# Grab image from stream
stream.seek(0)
img_data = stream.getvalue()
if self.uncompress:
# Publish raw image
data = np.fromstring(img_data, dtype=np.uint8)
image = cv2.imdecode(data, 1)
image_msg = self.bridge.cv2_to_imgmsg(image)
else:
# Publish compressed image only
image_msg = CompressedImage()
image_msg.data = img_data
image_msg.format = "jpeg"
image_msg.header.stamp = rospy.Time.now()
# Publish
publisher.publish(image_msg)
# Clear stream
stream.seek(0)
stream.truncate()
if not self.has_published:
rospy.loginfo("[%s] Published the first image." %(self.node_name))
self.has_published = True
def __init__(self):
# Get the ~private namespace parameters from command line or launch file.
self.UDP_IP = rospy.get_param('~UDP_IP', '192.168.50.37') #get_ip.get_local()
#if self.UDP_IP != 'localhost':
# self.UDP_IP = int(self.UDP_IP)
self.UDP_PORT = int(rospy.get_param('~UDP_PORT', '49155'))
# Create a dynamic reconfigure server.
#self.server = DynamicReconfigureServer(ConfigType, self.reconfigure)
# create ros::Publisher to send Odometry messages
cam_pub = rospy.Publisher('image_raw/compressed', CompressedImage)
#crappy test
t = True
while not rospy.is_shutdown():
#crappy test continued
if t:
print "Camera Running"
t = False
udp_string = self.receive_packet()
jpg_string = self.parse_string(udp_string)
cam_msg = CompressedImage()
cam_msg.header.stamp = rospy.Time.now() #YOYOYOYO - should be from robot time
cam_msg.format = "jpeg"
cam_msg.data = jpg_string
cam_pub.publish(cam_msg)
def grabFrame(event):
msg = CompressedImage()
msg.header.frame_id = frame_id
msg.format = "jpeg"
resp, msg.data = http.request("http://" + host + request)
msg.header.stamp = rospy.Time.now()
pub.publish(msg)
def ros_compressed_from_numpygray(np_arr):
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
np_arr = 255*(1.0*np_arr/np.max(np_arr))
msg.data = np.array(cv2.imencode('.jpg', np.array(np_arr, dtype = np.uint8))[1]).tostring()
return msg
def stream(self):
url = 'http://%s:%s@%s/mjpg/video.mjpg' % (self.axis.username, self.axis.password, self.axis.hostname)
#url = url + "?resolution=%dx%d" % (self.axis.width, self.axis.height)
fp = urllib.urlopen(url)
while True:
boundary = fp.readline()
header = {}
while 1:
line = fp.readline()
if line == "\r\n": break
line = line.strip()
parts = line.split(": ", 1)
header[parts[0]] = parts[1]
content_length = int(header['Content-Length'])
img = fp.read(content_length)
line = fp.readline()
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.axis_frame_id
msg.format = "jpeg"
msg.data = img
self.axis.pub.publish(msg)
"""
def grabAndPublish(self,stream,publisher):
while not self.update_framerate and not self.is_shutdown and not rospy.is_shutdown():
yield stream
# Construct image_msg
# Grab image from stream
stamp = rospy.Time.now()
stream.seek(0)
stream_data = stream.getvalue()
# Generate compressed image
image_msg = CompressedImage()
image_msg.format = "jpeg"
image_msg.data = stream_data
image_msg.header.stamp = stamp
image_msg.header.frame_id = self.frame_id
publisher.publish(image_msg)
# Clear stream
stream.seek(0)
stream.truncate()
if not self.has_published:
rospy.loginfo("[%s] Published the first image." %(self.node_name))
self.has_published = True
rospy.sleep(rospy.Duration.from_sec(0.001))
def publish(self, raw_img):
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode(".jpg", raw_img)[1]).tostring()
# TODO compile sensor_msg error , use String instead
self.image_pub.publish(msg.data)
def sendTestImage(self):
img_name = '0'+str(self.sent_image_count)+'_orig.'+self.test_image_format
test_img = cv2.imread(self.test_image_path+img_name)
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = self.test_image_format
msg.data = np.array(cv2.imencode('.'+self.test_image_format, test_img)[1]).tostring()
self.pub_test_image.publish(msg)
def pubOriginal(self, msg):
compressed_img_msg = CompressedImage()
compressed_img_msg.format = "png"
compressed_img_msg.data = np.array(cv2.imencode(".png", self.testImageOrig)[1]).tostring()
# compressed_img_msg.header.stamp = msg.header.stamp
# compressed_img_msg.header.frame_id = msg.header.frame_id
self.pub_raw.publish(compressed_img_msg)
def pubImage(self):
filename = '0'+str(self.num_sent)+'_orig.png'
image = cv2.imread(self.im_dir+filename)
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = 'png'
msg.data = np.array(cv2.imencode('.png', image)[1]).tostring()
self.pub_fake_images.publish(msg)
rospy.loginfo("[%s] Image %s published" %(self.node_name,filename))
def publish_image(pub, image_handle):
image = CompressedImage()
image.data = image_handle.read()
image.format = 'jpeg'
image.header.seq = 0
image.header.stamp.secs = math.floor(time.time())
image.header.stamp.nsecs = 0
image.header.frame_id = "0"
pub.publish(image)
def stream(self):
"""
Reads and process the streams from the camera
"""
try:
#If flag self.enable_auth is 'True' then use the user/password to access the camera. Otherwise use only self.url
if self.enable_auth:
req = urllib2.Request(self.url, None, {"Authorization": self.auth })
else:
req = urllib2.Request(self.url)
fp = urllib2.urlopen(req, timeout = self.timeout)
#fp = urllib2.urlopen(req)
while self.run_camera and not rospy.is_shutdown():
boundary = fp.readline()
header = {}
while not rospy.is_shutdown():
line = fp.readline()
#print 'read line %s'%line
if line == "\r\n": break
line = line.strip()
#print line
parts = line.split(": ", 1)
header[parts[0]] = parts[1]
content_length = int(header['Content-Length'])
#print 'Length = %d'%content_length
img = fp.read(content_length)
line = fp.readline()
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.axis_frame_id
msg.format = "jpeg"
msg.data = img
# publish image
self.compressed_image_publisher.publish(msg)
cimsg = self.cinfo.getCameraInfo()
cimsg.header.stamp = msg.header.stamp
cimsg.header.frame_id = self.axis_frame_id
# publish camera info
self.caminfo_publisher.publish(cimsg)
#print self.real_fps
self.last_update = datetime.datetime.now()
self.error_reading = False
self.image_pub_freq.tick()
except urllib2.URLError,e:
self.error_reading = True
self.error_reading_msg = e
rospy.logerr('Axis:stream: Camera %s (%s:%d). Error: %s'%(self.camera_id, self.hostname, self.camera_number, e))
def cv2_to_compressed_ros(self, img):
try:
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', img)[1]).tostring()
return msg
except CvBridgeError as e:
rospy.logerr(e)
def pubOrig(self, args=None):
image_msg_out = CompressedImage()
image_msg_out.header.stamp = rospy.Time.now()
image_msg_out.format = "png"
image_msg_out.data = np.array(cv2.imencode('.png',
self.original_image)[1]).tostring()
self.pub_image.publish(image_msg_out)
rospy.loginfo("Publishing original image")
def processImage(self, image_msg):
image_cv = self.bridge.imgmsg_to_cv2(image_msg)
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', image_cv)[1]).tostring()
self.pub_image_comp.publish(msg)
def stream(self):
url = 'http://%s/mjpg/video.mjpg' % self.axis.hostname
url = url + "?fps=0&resolultion=%dx%d" % (self.axis.width, self.axis.height)
rospy.logdebug('opening ' + str(self.axis))
# only try to authenticate if user/pass configured
if self.axis.password != '' and self.axis.username != '':
# create a password manager
password_mgr = urllib2.HTTPPasswordMgrWithDefaultRealm()
# Add the username and password, use default realm.
top_level_url = "http://" + self.axis.hostname
password_mgr.add_password(None, top_level_url,
self.axis.username,
self.axis.password)
handler = urllib2.HTTPBasicAuthHandler(password_mgr)
# create "opener" (OpenerDirector instance)
opener = urllib2.build_opener(handler)
# ...and install it globally so it can be used with urlopen.
urllib2.install_opener(opener)
fp = urllib2.urlopen(url)
while True:
boundary = fp.readline()
header = {}
while 1:
line = fp.readline()
if line == "\r\n": break
line = line.strip()
parts = line.split(": ", 1)
header[parts[0]] = parts[1]
content_length = int(header['Content-Length'])
img = fp.read(content_length)
line = fp.readline()
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.axis.frame_id
msg.format = "jpeg"
msg.data = img
self.axis.pub.publish(msg)
cimsg = self.axis.cinfo.getCameraInfo()
cimsg.header.stamp = msg.header.stamp
cimsg.header.frame_id = self.axis.frame_id
cimsg.width = self.axis.width
cimsg.height = self.axis.height
self.axis.caminfo_pub.publish(cimsg)
def flipImageNP(self,msg):
np_array = np.fromstring(msg.data, np.uint8)
image_np = cv2.imdecode(np_array, cv2.CV_LOAD_IMAGE_COLOR)
#flip_arr = np.fliplr(np_arr)
imageflip_np=cv2.flip(image_np,1)
flip_im = CompressedImage()
flip_im.data = np.array(cv2.imencode('.jpg', imageflip_np)[1]).tostring()
#flip_im.data = flip_arr.tostring()
flip_im.header.stamp = msg.header.stamp
self.pub_image.publish(flip_im)
def callback(self,ros_data):
np_arr = np.fromstring(ros_data.data, np.uint8)
image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
image_np = cv2.flip(image_np, self.flip_code)
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()
self.image_pub.publish(msg)
def receive_image(self):
"""
Retrives image data coming from the rover and then
transforms it to numpy and string data for further
manupulation with OpenCV and Kivy and returns the final
data
"""
data = ''
ldata = array.array('c')
image_buffer = CompressedImage()
image_buffer.format = "jpeg"
# start = ''
found_start = False
found_end = False
start_pos = 0 #current position in ldata
#position of the end of frame in the current array of data
end_pos = 0
while (not found_end):
data = self.video_socket.recv(self.max_tcp_buffer)
if(data == ''):
continue
#check for the message start token 'MO_V'
for i in range(0, len(data)-2):
if (data[i:(i+4)] == 'MO_V'):
if not found_start:
found_start = True
start_pos = i
#crop the data only include stuff after the
#start token.
data = data[start_pos:len(data)]
break
elif not found_end:
found_end = True
end_pos = i
break
#if you have found the start but not the end (in the
#middle of a image frame)
if (found_start and not found_end):
#add the recent data to ldata
ldata.extend(list(data))
if found_end:
ldata.extend(list(data[0:end_pos]))
data = ''
l_len = len(ldata)
#### Create CompressedIamge ####
# convert to numpy to use with OpenCV, etc.
image_buffer.header.stamp = rospy.Time.now()
image_buffer.data = np.array(ldata[36:l_len]).tostring()
return image_buffer
def __init__(self):
self.node_name = "Virtual Mirror Nbuckman Tester"
self.fake_pub_image = rospy.Publisher("/ernie/camera_node/image/compressed", CompressedImage,queue_size=1)
img_file = rospy.get_param("~img_file","/home/noambuckman/test_images/01_horz.jpg")
rospy.loginfo(img_file)
im_cv = cv2.imread(img_file)
img_str = cv2.imencode('.jpg', im_cv)[1].tostring()
pub_im=CompressedImage()
pub_im.header=rospy.Time.now()
pub_im.data = img_str
#pub_im.data = np.array(cv2.imencode('.jpg', im_np)[1]).tostring()
#flip_im.data = flip_arr.tostring()
#flip_im.header.stamp = msg.header.stamp
self.fake_pub_image.publish(pub_im)
def publish(self):
img = cv2.imread("/home/qian/dev/catkin_ws/src/smart_ar/script/aurora.png", cv2.CV_LOAD_IMAGE_COLOR)
M = cv2.getRotationMatrix2D((img.shape[1]/2,img.shape[0]/2),self.ga*100, 1.0 / self.dist)
img = cv2.warpAffine(img,M,(img.shape[1], img.shape[0]))
bridge = CvBridge()
msg = CompressedImage()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode(".jpg",img)[1]).tostring()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'image_compressed_publisher'
msg.header.seq = self.seq
self.seq += 1
self.imgPublisher.publish(msg)
print self.seq, 'image published'
def publishTestImage(self):
original_img = cv2.imread(self.original_image_file, cv2.CV_LOAD_IMAGE_COLOR)
#### Create CompressedIamge ####
out = CompressedImage()
out.header.stamp = rospy.Time.now()
out.format = "png"
np_arr_out = cv2.imencode('.png', original_img)[1]
out.data = np.array(np_arr_out).tostring()
# Publish new image
rospy.sleep(1)
self.pub_mirror.publish(out)
rospy.loginfo("[%s] published image"%self.node_name)
def cbImage(self, img):
# decode image
np_arr = np.fromstring(img.data, np.uint8)
color = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
self.overlayInfo(color)
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', color)[1]).tostring()
# Publish new image
self.image_pub.publish(msg)
def cropCallback(msg):
time_stamp = msg.header.stamp
img_bytes = BytesIO(msg.data)
img = PIL.Image.open(img_bytes)
resize = (80, 60)
img = img.resize(resize, resample=PIL.Image.BILINEAR)
np_img = np.asarray(img)
msg = CompressedImage()
msg.header.stamp = time_stamp
msg.format = "png"
msg.data = np.array(cv2.imencode('.png', np_img)[1]).tostring()
publish_cropimg.publish(msg)
def cam_callback(msg):
np_arr = np.fromstring(msg.data, np.uint8)
# Decode
image_rgb = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
# Mirror image laterally
image_rgb_rev = image_rgb[:,::-1,:]
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', image_rgb_rev)[1]).tostring()
publisher.publish(msg)
rospy.loginfo("Re Published ")
def set_scene(index):
image = CompressedImage()
# image.header.frame_id = '/odom_combined'
# If you ever want to drive the base, you will want to use the line above, or /map or something.
# Using /base_link means the robot will always stay exactly in the middle of the backdrop.
image.header.frame_id = '/base_link'
image.format = 'jpeg'
try:
scene = scenes[index]
except TypeError:
index = main_window.scene_comboBox.findText(index)
scene = scenes[index]
image.data = open(scene).read()
image.header.stamp = rospy.Time.now()
backdrop_publisher.publish(image)
def on_image(self, ros_data):
compressed_in = np.fromstring(ros_data.data, np.uint8)
image_in = cv2.imdecode(compressed_in, cv2.CV_LOAD_IMAGE_COLOR)
if self.prvs == None :
self.prvs = cv2.resize(cv2.cvtColor(image_in,cv2.COLOR_BGR2GRAY), (width,height))
else:
next = cv2.resize(cv2.cvtColor(image_in,cv2.COLOR_BGR2GRAY), (width,height))
image_out = cv2.resize(image_in, (width,height))
if self.should_be_controlled() : self.pub.publish(self.apply_control(next, image_out))
self.prvs = next
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', image_out)[1]).tostring()
self.ipub.publish(msg)
def _publish_image(self, header, image, timestamp):
"""Publish JPG image as a ROS message.
:param header: The HTTP-like header read from the stream.
:type header: dict
:param image: The video frame in JPG.
:type image: :py:obj:`basestring`
:param timestamp: The time when the frame was captured (or its best estimation).
:type timestamp: :py:class:`rospy.Time`
"""
msg = CompressedImage()
msg.header.stamp = timestamp
msg.header.frame_id = self._axis._frame_id
msg.format = "jpeg"
msg.data = image
self._axis._video_publisher.publish(msg)
def run(self):
cap = cv2.VideoCapture(1)
while (not rospy.is_shutdown()):
ret, self.image_np = cap.read()
t = time.time()
self.hsv = cv2.cvtColor(self.image_np, cv2.COLOR_BGR2HSV)
lower_blue = np.array([90, 30, 90])
upper_blue = np.array([250, 230, 250])
self.mask = cv2.inRange(self.hsv, lower_blue, upper_blue)
self.mask = cv2.morphologyEx(self.mask, cv2.MORPH_OPEN, np.ones((5,5), np.uint8))
M = cv2.moments(self.mask)
try:
cx = int(M["m10"]/M["m00"])
cy = int(M["m01"]/M["m00"])
except ZeroDivisionError:
cx = 0
cy = 0
self.centroid = (cx, cy)
cv2.circle(self.image_np, self.centroid, 5, (0, 0, 255), thickness=-1)
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', self.image_np)[1]).tostring()
self.image_pub.publish(msg)
def pub_record():
while True:
try:
global startflag
if startflag is True:
global submsg
if submsg != '':
#image000 = cv2.imread('/home/nvidia/workspace/src/detectAndRecog/src/yolo_surface/data/output.jpg')
global ipstream_dev1
if ipstream_dev1.isOpened():
_, img = ipstream_dev1.read()
global picId
cv2.imwrite(RecordPath + str(picId) + '.jpg', img)
msg = CompressedImage()
msg.data = np.array(cv2.imencode('.jpg',
img)[1]).tostring()
msg.header.frame_id = submsg + '/' + str(
picId) + '/' + str(subMultipId) + '-' + str(
subSingleId) + '-' + ResultLable
record_pub.publish(msg)
picId = int(picId) + 1
except:
pass
def __init__(self):
''' Initialize the server to enable the collection and publication of image and camera data. '''
rospy.init_node('image_server')
self.port = rospy.get_param('~port_number')
self.camera_name = rospy.get_param('~camera_name')
self.ios_clock_offset = 0
self.bridge = CvBridge()
self.clock_sub = rospy.Subscriber('/ios_clock', Float64,
self.handle_ios_clock)
self.pub_camera = rospy.Publisher('/' + self.camera_name +
'/image_raw/compressed',
CompressedImage,
queue_size=10)
self.pub_lowres = rospy.Publisher('/' + self.camera_name +
'_lowres/image_raw/compressed',
CompressedImage,
queue_size=10)
self.pub_camera_info = rospy.Publisher('/' + self.camera_name +
'/camera_info',
CameraInfo,
queue_size=10)
self.pub_lowres_info = rospy.Publisher('/' + self.camera_name +
'_lowres/camera_info',
CameraInfo,
queue_size=10)
self.cvmsg = CompressedImage()
self.image_data = {}
self.msg = CompressedImage()
self.intrinsic_msg = CameraInfo()
self.updated_intrinsics = None
self.last_packet_timestamp = rospy.Time(0.0)
UDP_IP = "0.0.0.0"
self.sock = socket.socket(
socket.AF_INET, #Internet
socket.SOCK_DGRAM) #UDP
self.sock.bind((UDP_IP, self.port))
def callback(self, ros_data):
# '''Callback function of subscribed topic.
# Here images get converted and features detected'''
# if VERBOSE :
# print 'received image of type: "%s"' % ros_data.format
#
# #### direct conversion to CV2 ####
# # np_arr = np.fromstring(ros_data.data, np.uint8)
# np_arr = np.fromstring(ros_data.data, np.uint8)
# image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
# #image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) # OpenCV >= 3.0:
#
#
#
# cv2.imshow('cv_img', image_np)
# cv2.waitKey(2)
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()
# Publish new image
self.image_pub.publish(msg)
def main(self):
print('start')
cam = cv2.VideoCapture(0)
cv2.namedWindow('publisher')
cv2.moveWindow('publisher', 40, 30)
while True:
ret_val, cv_image = cam.read()
#publising compressed image
msg_raw_image = CompressedImage()
msg_raw_image.header.stamp = rospy.Time.now()
msg_raw_image.format = "jpeg"
msg_raw_image.data = np.array(cv2.imencode(
'.jpg', cv_image)[1]).tostring()
self._pub1.publish(msg_raw_image)
#publishing raw image
self._pub2.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
cv2.imshow('publisher', cv_image)
key = cv2.waitKey(20) & 0xFF
if key != 255:
break
cam.release
cv2.destroyAllWindows()
exit()
def pub_img():
'''Callback function of subscribed topic.
Here images get converted and features detected'''
cap = cv2.VideoCapture(1)
print('Initilized started publishing')
while (True):
ret, frame = cap.read()
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', frame)[1]).tostring()
# Publish new image
pub = rospy.Publisher("/output/image_raw/compressed",
CompressedImage,
queue_size=1)
r = rospy.Rate(10)
try:
pub.publish(msg)
r.sleep()
except:
print('Could not publish')
def grab_and_publish(self, stream, publisher):
# Can't change parameters during sequence so may have to break out
while not self.needs_update and not rospy.is_shutdown():
yield stream
# Construct image_msg
# Grab image from stream
stamp = rospy.Time.now()
stream.seek(0)
stream_data = stream.getvalue()
# Generate compressed image
image_msg = CompressedImage()
image_msg.format = "jpeg"
image_msg.data = stream_data
image_msg.header.stamp = stamp
image_msg.header.frame_id = self.frame_id
publisher.publish(image_msg)
# Clear stream
stream.seek(0)
stream.truncate()
rospy.sleep(0.001) # 1ms
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" % (self.node_name))
self.framerate_high = self.setupParam("~framerate_high", 30.0)
self.framerate_low = self.setupParam("~framerate_low", 15.0)
self.res_w = self.setupParam("~res_w", 640)
self.res_h = self.setupParam("~res_h", 480)
self.image_msg = CompressedImage()
# Setup PiCamera
self.camera = PiCamera()
self.framerate = self.framerate_high # default to high
self.camera.framerate = self.framerate
self.camera.resolution = (self.res_w, self.res_h)
# For intrinsic calibration
rospack = rospkg.RosPack()
self.cali_file_folder = rospack.get_path('pi_camera') + "/config/"
self.frame_id = rospy.get_namespace().strip(
'/') + "/camera_optical_frame"
self.has_published = False
self.pub_img = rospy.Publisher("~image/compressed",
CompressedImage,
queue_size=1)
self.sub_switch_high = rospy.Subscriber("~framerate_high_switch",
Bool,
self.cbSwitchHigh,
queue_size=1)
# Create service (for camera_calibration)
self.srv_set_camera_info = rospy.Service("~set_camera_info",
SetCameraInfo,
self.cbSrvSetCameraInfo)
self.stream = io.BytesIO()
#self.camera.exposure_mode = 'off'
# self.camera.awb_mode = 'off'
self.is_shutdown = False
self.update_framerate = False
# Setup timer
rospy.loginfo("[%s] Initialized." % (self.node_name))
def execute_cb(self, goal):
rospy.loginfo("Goal Received!")
start_time = rospy.Time.now()
result = inference_server.msg.InferenceResult()
if not goal.input_image.data:
self.inference_output, num_detected, detected_classes, detected_scores, detected_boxes = object_detection.detect(
self.inference_input)
else:
np_arr = np.fromstring(goal.input_image.data, np.uint8)
goal_inference_input = cv2.imdecode(np_arr,
cv2.CV_LOAD_IMAGE_COLOR)
self.inference_output, num_detected, detected_classes, detected_scores, detected_boxes = object_detection.detect(
goal_inference_input)
self.inference_image = CompressedImage()
self.inference_image.header.stamp = rospy.Time.now()
self.inference_image.format = "jpeg"
self.inference_image.data = np.array(
cv2.imencode('.jpg', self.inference_output)[1]).tostring()
self.image_pub.publish(self.inference_image)
result.image = self.inference_image
result.num_detections = num_detected
result.classes = detected_classes
result.scores = detected_scores
for i in range(len(detected_boxes)):
box = RegionOfInterest()
box.y_offset = detected_boxes[i][0]
box.height = (detected_boxes[i][2] - detected_boxes[i][0])
box.x_offset = detected_boxes[i][1]
box.width = (detected_boxes[i][3] - detected_boxes[i][1])
box.do_rectify = True
result.bounding_boxes.append(box)
total_inference_time = rospy.Time.now() - start_time
total_inference_time = total_inference_time.to_sec()
rospy.loginfo(
"The inference took {} seconds".format(total_inference_time))
try:
self._as.set_succeeded(result)
except Exception as e:
rospy.logerr(str(e))
self._as.set_aborted(text=str(e))
def __init__(self):
"""OpenCV feature detectors with ros CompressedImage Topics in python.
This example subscribes to a ros topic containing sensor_msgs
CompressedImage. It converts the CompressedImage into a numpy.ndarray,
then detects and marks features in that image. It finally displays
and publishes the new image - again as CompressedImage topic.
"""
self.publisher = rospy.Publisher("/image_raw/compressed",
CompressedImage,
queue_size=4)
self.msg = CompressedImage()
rospy.loginfo('Setting camera level...')
self.camera = cv2.VideoCapture(int(rospy.get_param('camera_port', 0)))
self.takeImage(lightSettingSnaps=30)
def gen_and_send(self):
ts = time.time()
data = []
w = 32
nvecs = w * w
for i in range(w):
for j in range(w):
v = (math.sin(i / 16.0 + ts / 3.0) +
math.cos(j / 16.0 + ts / 3.0)) * 0.1
data += [int(v * self.SCALE_FACTOR)]
rvl.Clear()
rvl.plain = data
rvl.CompressRVL(chan=0)
msg = CompressedImage(header=Header(
frame_id=self.FRAME_ID, stamp=self.get_clock().now().to_msg()),
format="RVL",
data=rvl.encoded)
self.pub.publish(msg)
def __init__(self, _starttime):
self.floorcam_subscriber = rospy.Subscriber('/pi_floorcam/image_raw/compressed',
CompressedImage, self.callback_floorcam)
self.pub_box = rospy.Publisher(
'/pi_floorcam_box/image_raw/compressed', CompressedImage, queue_size=3)
#initialize
self.np_arr = np.empty(0)
self.image_cv = np.zeros((0,0,3), np.uint8)
self.image_cv_box = np.zeros((0,0,3), np.uint8)
self.image_cv_gray = np.zeros((0,0,1), np.uint8)
self.msg_box = CompressedImage()
self.msg_box.format = "jpeg"
self.imageReceived = False
self.r = rospy.Rate(10) #10Hz
# self.save_mode = False
self.isStarted = False
self.average_box = 0.0
def publish_rvl(self):
now = self.robot.getTime()
if now - self.lastUpdate < (self.range.getSamplingPeriod() / 1000.0):
return
self.robot.step(self.timestep)
rvl.Clear()
# Remap to 16-bit integer
raw = self.range.getRangeImage()
if raw is None:
self.get_logger().info("No range image", throttle_duration_sec=3)
return
rvl.plain = [int(65535 * px / self.MAX_RANGE) for px in raw]
rvl.CompressRVL(chan=0)
header = Header(frame_id=self.FRAME_ID,
stamp=Time(sec=int(now),
nanosec=int((now - int(now)) * 1.0e+6)))
msg = CompressedImage(header=header, format="RVL", data=rvl.encoded)
self.pub.publish(msg)
self.lastUpdate = self.robot.getTime()
def __init__(self, pipeline, image_pub):
self.image_pub = image_pub
self.pipeline = Gst.parse_launch(pipeline)
if self.pipeline is None:
raise PipelineException(
'Failed to parse pipeline: {}'.format(pipeline))
self.sink = self.pipeline.get_by_name('sink')
if self.sink is None:
raise PipelineException(
'Could not find an element named "sink" in pipeline: {}'.
format(pipeline))
self.sink.set_property('emit-signals', True)
self.sink.set_property('sync', False)
self.sink.set_property('max-buffers', 2)
self.sink.set_property('drop', True)
self.sink.connect('new-sample', self._publish_sample, None)
self.image_msg = CompressedImage()
def __init__(self,
calibration_object,
publisher_compressed_stream,
flip_img=False):
super(GazeboStream,
self).__init__(calibration_object, publisher_compressed_stream,
flip_img)
self.__image_raw_sub = rospy.Subscriber('/gazebo_camera/image_raw',
Image,
self.__callback_sub_image_raw)
self.__image_compressed_sub = rospy.Subscriber(
'/gazebo_camera/image_raw/compressed', CompressedImage,
self.__callback_sub_image_compressed)
self.__bridge = CvBridge()
self.__last_image_raw = None
self.__last_image_compressed_msg = CompressedImage()
self._running = True
self._should_run = True
def __init__(self, publish_intrinsic_info, camera_info,
camera_info_intrinsics):
self.done = False
self.pub_img = rospy.Publisher("~image/compressed",
CompressedImage,
queue_size=1)
self.pub_camera_info = rospy.Publisher("~camera_info",
CameraInfo,
queue_size=1)
self.pub_camera_info_intrinsics = rospy.Publisher(
"~camera_info_intrinsic_calibration", CameraInfo, queue_size=1)
self.camera_info = camera_info
self.camera_info_intrinsics = camera_info_intrinsics
self.msg_img = CompressedImage()
self.stream = io.BytesIO()
self.publish_intrinsic_info = publish_intrinsic_info
def initCamera(self):
self.get_logger().info('Initializing Camera ')
self.image_msg = CompressedImage()
#load parameter
self.framerate = self.parameters["BasicParam"]["framerate"]
self.res_width = self.parameters["BasicParam"]["res_width"]
self.res_height = self.parameters["BasicParam"]["res_height"]
self.vertical_flip = self.parameters["BasicParam"]["vertical_flip"]
self.horizontal_flip = self.parameters["BasicParam"]["horizontal_flip"]
#create Picamera object
self.picamera = PiCamera()
self.picamera.resolution = (self.res_width, self.res_height)
self.picamera.vflip = self.vertical_flip
self.picamera.hflip = self.horizontal_flip
self.picamera.framerate = self.framerate
self.compress_stream = io.BytesIO()
self.get_logger().info('Initialized Camera ')
def talker():
pub = rospy.Publisher('cam_data', CompressedImage, queue_size=10)
pub_enc = rospy.Publisher('cam_data_enc', CompressedImage, queue_size=10)
rospy.init_node('drone_cam', anonymous=True)
# rate = rospy.Rate(50) # 10hz
# a = ""
# for i in range(0,size):
# a = a + "a"
# rospy.loginfo(sys.getsizeof(a))
while not rospy.is_shutdown():
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.zeros((10, 10)).tostring()
pub.publish(msg)
msg_enc = CompressedImage()
msg_enc.header.stamp = rospy.Time.now()
msg_enc.format = "jpeg"
msg_enc.data = encrypt(
Padding.appendPadding(np.zeros((10, 10)).tostring(), mode='CMS'),
key, AES.MODE_CBC, salt)
pub_enc.publish(msg_enc)
def subscribe_robot_data(self):
rospy.loginfo("Subscribing to robot data.")
# Reset variables
self.reset_variables()
# Create image buffers
self.raw_image_buffer = collections.deque(self.image_buffer_size*[Image()], self.image_buffer_size)
self.compressed_image_buffer = collections.deque(self.image_buffer_size*[CompressedImage()], self.image_buffer_size)
self.last_raw_image_time = rospy.Time.now()
self.last_compressed_image_time = rospy.Time.now()
# Create robot data subscribers
self.robot_data_subscribers = []
self.robot_data_subscribers.append(rospy.Subscriber(self.robot_mode_topic, RobotMode, self.robot_mode_callback))
self.robot_data_subscribers.append(rospy.Subscriber(self.gps_fix_topic, NavSatFix, self.navsatfix_callback))
self.robot_data_subscribers.append(rospy.Subscriber(self.local_pose_topic, Pose, self.pose_callback))
self.robot_data_subscribers.append(rospy.Subscriber(self.map_topic, OccupancyGrid, self.map_callback))
self.robot_data_subscribers.append(rospy.Subscriber(self.image_topic, Image, self.image_callback))
self.robot_data_subscribers.append(rospy.Subscriber(self.image_compressed_topic, CompressedImage, self.compressed_image_callback))
# If using tf, create tf listener for local position
if self.use_tf:
self.tfBuffer = tf2_ros.Buffer(rospy.Duration(self.send_pose_period))
self.listener = tf2_ros.TransformListener(self.tfBuffer)
pass
def pub_single_compressed_image(self):
print "BUFFER 1"
if self.cur_image_i < self.total_msg_count: # if not all the messages are sent yet.
print "BUFFER 2"
print self.cur_image_i
view_output = next(self.view_generator) # view_output class : <class 'rosbag.bag.BagMessage'>, has return value (topic_name, msg, time_stamp of the message)
# message generation
msg = CompressedImage() # create a compressed image object to publish
msg = view_output.message # message content of the view is what we want to publish
rospy.loginfo("[{}] publishing image {}".format(self.node_name, self.cur_image_i))
self.pub_compressed_image.publish(msg) # publish the message
self.cur_image_i += 1
print "BUFFER 3"
else: # after sending all messages close the bag
if self.send_out_all_images == False:
rospy.loginfo('[{}] send all the messages'.format(self.node_name))
self.input_rosbag.close()
self.send_out_all_images = True
self.send_status = rospy.set_param(self.pm_send_status,1)
else:
print("!!!!!!!!!! SHOULD NOT BE HERE 2 ")
def __init__(self):
super().__init__('acapture_camera_node',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True)
self.node_name = self.get_name()
self.log = self.get_logger()
self.cap = acapture.open(0) #/dev/video0
#self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
#self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
#self.cap.set(cv2.CAP_PROP_FPS, 5)
self.log.info("successfully opened camera device!")
# init ROS opencv bridge
self.bridge = CvBridge()
self.log.info("sucessfully created CvBridge.")
self.pub_img = self.create_publisher(CompressedImage,
"image/compressed", 1)
self.image_msg = CompressedImage()
def __init__(self, main=None):
self.main = main
self.detected = False
self.captured_image = CompressedImage()
self.camera_info = CameraInfo()
self.detection_data = AprilTagDetectionArray()
self.best_score = -1
self.best_idx = -1
self.best_position = Point()
# Register listener for detection.
rospy.wait_for_message("tag_detections", AprilTagDetectionArray)
rospy.Subscriber("tag_detections", AprilTagDetectionArray, self.get_detect_data)
# Register listener for capture.
rospy.wait_for_message("/camera/rgb/image_color/compressed", CompressedImage)
rospy.Subscriber("/camera/rgb/image_color/compressed", CompressedImage, self.get_capture_data)
# Register listener for camera info.
rospy.wait_for_message("/camera/rgb/camera_info", CameraInfo)
rospy.Subscriber("/camera/rgb/camera_info", CameraInfo, self.get_camera_info)
def callback(self, ros_data):
'''Callback function of subscribed topic.
Here images get converted and encrypted'''
# DEBUG:
# print 'Received image of type: "%s"' % ros_data.format
#### direct conversion to CV2 ####
np_arr = np.fromstring(ros_data.data, np.uint8)
image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
font = cv2.FONT_HERSHEY_SIMPLEX
image_np_with_text = cv2.copyMakeBorder(image_np, 0, 0, 0, 0,
cv2.BORDER_REPLICATE)
#cv2.putText(image_np_with_text,"{:.2f}".format(self.fps.partialfps()),(10,470), font, 1,(255,255,255),2)
#cv2.imshow('Before_Encryption', image_np_with_text)
#cv2.waitKey(2)
#self.fps.update()
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()
if (self.enc_mode == MODE_3DES):
msg.data = self.encrypt_chunk_3DES(msg.data, self.key_3DES,
self.iv_3DES)
elif (self.enc_mode == MODE_AES):
msg.data = b64encode(msg.data)
plaintext_padded = self.AddPadding(msg.data, INTERRUPT, PAD,
BLOCK_SIZE)
encrypted = self.encrypt_chunk_AES(plaintext_padded)
msg.data = b64encode(encrypted)
# Publish new image
self.image_pub.publish(msg)
def __init__(self, json_path, weights_path):
self.img = CompressedImage()
self.grayimg = None
self.gray_resize = None
self.img_size = (200, 200)
self.img_sub = rospy.Subscriber(
'/zed/zed_node/left/image_rect_color/compressed', CompressedImage,
self.img_callback)
# DroNet
self.config = tf.ConfigProto()
self.config.gpu_options.allow_growth = True
self.config.log_device_placement = True
self.sess = tf.Session(config=self.config)
self.setsess = set_session(self.sess)
self.loaded_json = open(json_path, 'r')
self.loaded_model = self.loaded_json.read()
self.model = keras.models.model_from_json(self.loaded_model)
self.model.load_weights(weights_path)
self.isGet = False
"""
def get_messages_filled_with_data(robot_name):
array_msg = ArtifactArray()
compressed_img_msg = CompressedImage()
artifact_img_msg = ArtifactImg()
array_msg.header.seq = 1
array_msg.header.stamp = rospy.Time.now()
array_msg.header.frame_id = ''
array_msg.owner = robot_name
id = 0
for i in range(1, 20):
id += 1
array_msg.artifacts.append(make_message(id, i))
cv_img = cv2.imread(ARTIFACT_IMG_PATH, cv2.IMREAD_COLOR)
compressed_img_msg = CvBridge().cv2_to_compressed_imgmsg(cv_img)
artifact_img_msg.header.stamp = rospy.Time.now()
artifact_img_msg.artifact_img = compressed_img_msg
artifact_img_msg.artifact_id = str(id)
return array_msg, compressed_img_msg, artifact_img_msg
def __init__(self, filename, frame_id, camera_info_url, prefix):
self.filename = filename
self.frame_id = frame_id
self.camera_info_url = camera_info_url
self.msg = CompressedImage()
self.msg.header.frame_id = self.frame_id
self.msg.format = "jpeg"
im = cv2.imread(filename)
self.width=im.shape[1]
self.height=im.shape[0]
f=open(filename, 'r+')
self.msg.data = f.read()
self.valid = True;
self.updated = True;
# generate a valid camera name based on the hostname
self.cname = camera_info_manager.genCameraName(filename)
self.cinfo = camera_info_manager.CameraInfoManager(cname = self.cname,
url = self.camera_info_url, namespace = prefix)
self.cinfo.loadCameraInfo() # required before getCameraInfo()
self.st = None
self.pub = rospy.Publisher(prefix + "/image_raw/compressed", CompressedImage, self)
self.caminfo_pub = rospy.Publisher(prefix + "/camera_info", CameraInfo, self)
detector = SignDetector((model_loc, model_det))
from sensor_msgs.msg import Image
from sensor_msgs.msg import CompressedImage
from std_msgs.msg import String
from cv_bridge import CvBridge, CvBridgeError
sign_pub = rospy.Publisher("sign_name", String, queue_size=10)
img_pub = rospy.Publisher("result_image/compressed", CompressedImage, queue_size=10)
# img_pub = rospy.Publisher("result_image", Image, queue_size=1)
bridge = CvBridge()
msgImg = CompressedImage()
msgImg.format = "jpeg"
class ImageProcessor:
def __init__(self):
rospy.Subscriber("sign_image", Image, self.callback, queue_size=1)
self.cv_image = None
def callback(self, data):
try:
self.cv_image = bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
def print_fps(fps):
def callback(self, ros_data):
aruco = cv2.aruco
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
arucoMarkerLength = 0.096
PI = 3.1415
self.cameraMatrix = np.asarray([[255.64885718, 0., 163.44315533],
[0., 255.85790733, 118.62161079],
[0., 0., 1.]])
self.distanceCoefficients = np.asarray(
[0.20120402, -0.43881712, 0.00463485, 0.00622489, 0.12081623])
'''Callback function of subscribed topic.
Here images get converted and features detected'''
if VERBOSE:
print 'received image of type: "%s"' % ros_data.format
#### direct conversion to CV2 ####
np_arr = np.fromstring(ros_data.data, np.uint8)
# image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) # OpenCV >= 3.0:
# hsv_img = cv2.cvtColor(image_np, cv2.COLOR_BGR2HSV)
# thresholded_img = cv2.inRange(hsv_img, (35, 80, 110), (60, 255, 255),)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
image_np, dictionary)
aruco.drawDetectedMarkers(image_np, corners, ids, (0, 255, 0))
if len(corners) > 0:
self.rvec, self.tvec, _ = aruco.estimatePoseSingleMarkers(
corners[0], arucoMarkerLength, self.cameraMatrix,
self.distanceCoefficients)
image_np = aruco.drawAxis(image_np, self.cameraMatrix,
self.distanceCoefficients, self.rvec,
self.tvec, 0.1)
# (roll_angle, pitch_angle, yaw_angle) = self.rvec[0][0][0], self.rvec[0][0][1], self.rvec[0][0][2]
# if pitch_angle < 0:
# roll_angle, pitch_angle, yaw_angle = -roll_angle, -pitch_angle, -yaw_angle
# print (roll_angle, pitch_angle, yaw_angle)
# print(self.tvec)
_quat = tf.transformations.quaternion_from_euler(
-PI / 2.0, -PI, PI / 2.0)
# _quat = tf.transformations.quaternion_from_euler(-PI / 2.0 + (roll_angle + PI), -PI + pitch_angle, PI / 2.0 + yaw_angle)
# _quat = (0., 0., 0., 1.)
self._broadcaster.sendTransform(
(self.tvec[0][0][2], self.tvec[0][0][0], self.tvec[0][0][1]),
_quat, rospy.Time.now(), "ar", "camera_face")
# self._broadcaster.sendTransform((-0, 0, 0), _quat, rospy.Time.now(), "base_link", "ar")
self.servo[0] -= 90.0 - math.degrees(
math.atan2(self.tvec[0][0][2], self.tvec[0][0][0]))
self.servo[1] += 90.0 - math.degrees(
math.atan2(self.tvec[0][0][2], self.tvec[0][0][1]))
self.servo[1] = Constrain(self.servo[1], 110, 30)
self.servo[0] = Constrain(self.servo[0], 135, 45)
self._camera_pub.publish(
str(self.servo[0]) + ", " + str(self.servo[1]) + ",")
# print(self.servo)
# cv2.imshow('cv_img', image_np)
# cv2.waitKey(2)
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()
# Publish new image
self.image_pub.publish(msg)
scld = cv2.resize(crop,None,fx=0.4,fy=1.2,interpolation=cv2.INTER_AREA)
# 3. Color transform ---------------------------------------------------
color = cv2.cvtColor(scld, cv2.COLOR_BGR2GRAY)
hght, wdth = color.shape[:2]
print(hght, wdth)
# 4. Inverse Prespective Mapping ---------------------------------------
pts1 = np.float32([[wdth/2-55,0],[wdth/2+55,0],[0,hght],[wdth,hght]])
pts2 = np.float32([[0,0],[wdth,0],[wdth/2-40,hght],[wdth/2+40,hght]])
M = cv2.getPerspectiveTransform(pts1,pts2)
dst = cv2.warpPerspective(color,M,(wdth,hght))
# Compress image to pub ------------------------------------------------
cropImage = CompressedImage()
cropImage.header.stamp = rospy.Time.now()
cropImage.format = "jpeg"
cropImage.data = np.array(cv2.imencode('.jpg',dst)[1]).tostring()
pub.publish(cropImage)
# Print stats ----------------------------------------------------------
e2 = cv2.getTickCount()
t = (e2 - e1)/cv2.getTickFrequency()
print('frame time:'+str(t)+'-------------------------------block end')
def main():
global pub
import cv2
from subprocess import Popen, PIPE
from shlex import split
from util import RobotUtil
# ROS Libraries
import rospy
import roslib
from sensor_msgs.msg import CompressedImage
VERBOSE = True
# CompressedImage Message Setup
msg = CompressedImage()
msg.format = 'png'
def image_pub():
# ROS Node setup
if VERBOSE:
print('Starting image_pub node...')
client = RobotUtil.VideoStreamClient(VERBOSE=VERBOSE, BGR2RGB=True)
client.start()
pub = rospy.Publisher('/PI_CAM/image_raw/compressed', CompressedImage)
rospy.init_node('image_pub', anonymous=True)
rate = rospy.Rate(100) # 100 Hz to prevent aliasing of 40 FPS feed
