def publish(self):
fn = '{}::publish()'.format(self.classname)
try:
start_time = time.time()
pub = rospy.Publisher('camera/image', Image, queue_size=1)
while not rospy.is_shutdown():
if self.videothread != None:
ret = False
img = self.videothread.frame
if (type(img) is np.ndarray):
ret = True
else:
ret, img = self.camera.read()
if ret == False:
msg='{}: Failed!!! camera read Failed!!! ret:{}'\
.format(fn, ret)
if time.time() - start_time > 5.0:
rospy.logerr('{}: failed!!! ret=False'.format(fn))
rospy.Rate(30).sleep() # 30 Hz
continue
#rospy.loginfo('{}: count:{}'.format(fn, self.count))
self.count += 1
img[0, 0, 0] = self.count
imgmsg = self.bridge.cv2_to_imgmsg(img, "bgr8")
#rospy.loginfo('{}: img[0,0,0]:{}'.format(fn, img[0,0,0]))
pub.publish(imgmsg)
rospy.Rate(self.frate).sleep() # 30 Hz
except Exception as err:
msg = '{}: Failed!!! line:{} err:{}'.format(
fn, myerror.lineno(), err)
rospy.logerr(msg)
raise ValueError(msg)
def fromvideofile(fname):
''' Capture image from video file and find lines'''
fn = 'fromframe()'
log = mylog.configure_logging('fromvideofile', "/tmp/findlines_algo.log")
try:
log.info('{}: START'.format(fn))
#log.info('{}: ##############################'.format(fn))
#log.info('{}: \tfnamearr:{}'.format(fn, fnamearr))
#log.info('{}: ##############################'.format(fn))
obj = AlgoFindLines(log=log, isfromros=False)
cap = cv.VideoCapture(fname)
while True:
ret, img = cap.read()
if ret != True:
log.error('{}: cap.read() Failed!!!'.format(fn))
continue
imgout = obj.find_line(img)
#if out_file !=None:
# out_file.write(normalizedFrame)
cv.imshow('frame', imgout)
if cv.waitKey(5) & 0xFF == ord('q'):
break
except Exception as err:
msg = '{}: Failed!!! line:{} err:{}'.format(fn, myerror.lineno(), err)
log.error(msg)
def start_node(self):
fn = '{}::start_node()'.format(self.classname)
try:
rospy.init_node('camerapub')
rospy.loginfo('image_pub node started')
except Exception as err:
msg = '{}: Failed!!! line:{} err:{}'.format(
fn, myerror.lineno(), err)
rospy.logerr(msg)
raise ValueError(msg)
def __init__(self):
self.classname = 'ROSDetectObj'
fn = '{}::__init_()'.format(self.classname)
try:
self.bridge = CvBridge()
self.algo = detectobj_algo.AlgoDetectObj(isfromros=True)
self.pubtopic = 'detectobj/image'
self.cmd = 'PAUSEPUB'
#
# Declare node
#
self.init_node()
except Exception as err:
msg = '{}: Failed!!! line:{} err:{}'.format(
fn, myerror.lineno(), err)
rospy.logerr(msg)
raise ValueError(msg)
def __init__(self):
self.classname = 'ROSFindLines'
fn = '{}::__init_()'.format(self.classname)
try:
self.bridge = CvBridge()
self.algo = findlines_algo.AlgoFindLines(isfromros=True)
self.pubtopic = 'findlines/image'
self.cmd = 'RESUMEPUB'
#
# Declare node
#
self.init_node()
except Exception as err:
msg = '{}: Failed!!! line:{} err:{}'.format(
fn, myerror.lineno(), err)
rospy.logerr(msg)
raise ValueError(msg)
def callback_newtopic(self, data):
fn = '{}::callback_newtopic()'.format(self.classname)
#rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
try:
rospy.loginfo('{}: get_caller_id:{} heard:{}'\
.format(fn, rospy.get_caller_id(), data.data))
pubtopic = data.data
if pubtopic != self.pubtopic:
self.cmd = 'PAUSEPUB'
rospy.loginfo('{}: PAUSEPUB'.format(fn))
else:
self.cmd = 'RESUMEPUB'
rospy.loginfo('{}: RESUMEPUB'.format(fn))
except Exception as err:
msg = '{}: Failed!!! line:{} err:{}'.format(
fn, myerror.lineno(), err)
rospy.logerr(msg)
raise ValueError(msg)
def init_node(self):
fn = '{}::init_node()'.format(self.classname)
try:
rospy.loginfo("{}: BR init_node()".format(fn))
rospy.init_node('detectobj', anonymous=True)
#
# Reroute the log
#
rospy.loginfo("{}: AR init_node()".format(fn))
self.count = 0
rospy.loginfo('{}: BR Subscriber to /camera/image'.format(fn))
#
# Subscriber
# queu_size=1 and buff_size=2**24 are necessary for retrieve only latest message(image)
#
#
rospy.Subscriber("camera/image",
Image,
self.callback,
queue_size=1,
buff_size=2**24)
rospy.loginfo('{}: BR Publisher of topic: {}'.format(
fn, self.pubtopic))
self.pub = rospy.Publisher(self.pubtopic, Image, queue_size=1)
#
# Subscribe also to a topic that switches video input to MJPEG
# If the message on /rpiwebserver/newtopic/ request video NOT from /findlines/image
# then stop publishing the video
#
self.subnewtopic = rospy.Subscriber("rpiwebserver/newtopic",
String, self.callback_newtopic)
# spin() simply keeps python from exiting until this node is stopped
rospy.loginfo('{}: BR spin'.format(fn))
rospy.spin()
except Exception as err:
msg = '{}: Failed!!! line:{} err:{}'.format(
fn, myerror.lineno(), err)
rospy.logerr(msg)
raise ValueError(msg)
def callback(self, data):
#rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
fn = '{}::callback()'.format(self.classname)
try:
#rospy.loginfo(rospy.get_caller_id() + "I heard image: {}".format(self.count))
if self.cmd == 'RESUMEPUB':
img = self.bridge.imgmsg_to_cv2(data, "bgr8")
#
# Find lines and publish
#
imgout = self.algo.find_line(img)
imgmsg = self.bridge.cv2_to_imgmsg(imgout, "bgr8")
self.pub.publish(imgmsg)
#cv.imwrite("{}/frame-{}.png".format('.', self.count), cv_image)
self.count += 1
except Exception as err:
msg = '{}: Failed!!! line:{} err:{}'.format(
fn, myerror.lineno(), err)
rospy.logerr(msg)
raise ValueError(msg)
def fromframefile(fname):
''' Capture image from frame and find lines'''
fn = 'fromframefile()'
log = mylog.configure_logging('fromframefile', "/tmp/findlines_algo.log")
try:
log.info('{}: START'.format(fn))
#log.info('{}: ##############################'.format(fn))
#log.info('{}: \tfnamearr:{}'.format(fn, fnamearr))
#log.info('{}: ##############################'.format(fn))
obj = AlgoFindLines(log=log, isfromros=False)
frame = cv.imread(fname)
imgout = obj.find_line(frame)
cv.imshow('frame', imgout)
#if cv.waitKey(5) & 0xFF == ord('q'):
# return
cv.waitKey(0)
except Exception as err:
msg = '{}: Failed!!! line:{} err:{}'.format(fn, myerror.lineno(), err)
log.error(msg)
def __init__(self, fname='0'):
self.classname = "CameraPublish"
fn = '{}::__init__()'.format(self.classname)
try:
self.count = 0
self.fname = None
self.frate = 30.0
#self.frate=4.0
self.start_node()
rospy.loginfo('{}: fname:{}'.format(fn, fname))
self.videothread = None
self.videothread = CameraGrabFrameThread(fname)
thread = threading.Thread(target=self.videothread.process)
thread.start()
rospy.loginfo('{}: AR create VideoCapture'.format(fn))
self.bridge = CvBridge()
except Exception as err:
msg = '{}: Failed!!! line:{} err:{}'.format(
fn, myerror.lineno(), err)
rospy.logerr(msg)
raise ValueError(msg)
