def __init__(self):
self.name = 'static_object_detector_node'
self.tm = Matcher()
self.active = False
self.thread_lock = threading.Lock()
self.turn_counter = TurnCounter()
self.pub_ibvs = rospy.Publisher("~ibvs", Float32, queue_size=1)
self.sub_image = rospy.Subscriber("~image_compressed", CompressedImage, self.cbImage, queue_size=1)
self.pub_image = rospy.Publisher("~servo_image", Image, queue_size=1)
self.pub_turns = rospy.Publisher("~turned", Bool, queue_size=1)
self.bridge = CvBridge()
rospy.loginfo("[%s] Initialized." %(self.name))
class StaticObjectDetectorNode:
def __init__(self):
self.name = 'static_object_detector_node'
self.tm = Matcher()
self.active = False
self.thread_lock = threading.Lock()
self.turn_counter = TurnCounter()
self.pub_ibvs = rospy.Publisher("~ibvs", Float32, queue_size=1)
self.sub_image = rospy.Subscriber("~image_compressed",
CompressedImage,
self.cbImage,
queue_size=1)
self.pub_image = rospy.Publisher("~servo_image", Image, queue_size=1)
self.pub_turns = rospy.Publisher("~turned", Bool, queue_size=1)
self.bridge = CvBridge()
rospy.loginfo("[%s] Initialized." % (self.name))
def cbSwitch(self, switch_msg):
self.active = switch_msg.data
def cbImage(self, image_msg):
thread = threading.Thread(target=self.processImage, args=(image_msg, ))
thread.setDaemon(True)
thread.start()
def processImage(self, image_msg):
if not self.thread_lock.acquire(False):
return
np_arr = np.fromstring(image_msg.data, np.uint8)
#image_cv=self.bridge.imgmsg_to_cv2(image_msg,"bgr8")
image_cv = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
img, center = self.tm.contour_match(image_cv)
crossing, turns = self.turn_counter.cbmsg(center)
if crossing:
# only trigger if it's been awhile
rospy.loginfo("Crossing. %d turn" % turns)
self.pub_turns.publish(Bool(data=True))
self.pub_ibvs.publish(Float32(data=center))
height, width = img.shape[:2]
"""
try:
self.pub_image.publish(self.bridge.cv2_to_imgmsg(img, "bgr8"))
except CvBridgeError as e:
print(e)
"""
self.thread_lock.release()
class StaticObjectDetectorNode:
def __init__(self):
self.name = 'static_object_detector_node'
self.tm = Matcher()
self.active = False
self.thread_lock = threading.Lock()
self.turn_counter = TurnCounter()
self.pub_ibvs = rospy.Publisher("~ibvs", Float32, queue_size=1)
self.sub_image = rospy.Subscriber("~image_compressed", CompressedImage, self.cbImage, queue_size=1)
self.pub_image = rospy.Publisher("~servo_image", Image, queue_size=1)
self.pub_turns = rospy.Publisher("~turned", Bool, queue_size=1)
self.bridge = CvBridge()
rospy.loginfo("[%s] Initialized." %(self.name))
def cbSwitch(self,switch_msg):
self.active = switch_msg.data
def cbImage(self,image_msg):
thread = threading.Thread(target=self.processImage,args=(image_msg,))
thread.setDaemon(True)
thread.start()
def processImage(self, image_msg):
if not self.thread_lock.acquire(False):
return
np_arr = np.fromstring(image_msg.data, np.uint8)
#image_cv=self.bridge.imgmsg_to_cv2(image_msg,"bgr8")
image_cv = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
img, center = self.tm.contour_match(image_cv)
crossing, turns = self.turn_counter.cbmsg(center)
if crossing:
# only trigger if it's been awhile
rospy.loginfo("Crossing. %d turn" % turns)
self.pub_turns.publish(Bool(data=True))
self.pub_ibvs.publish(Float32(data=center))
height,width = img.shape[:2]
"""
try:
self.pub_image.publish(self.bridge.cv2_to_imgmsg(img, "bgr8"))
except CvBridgeError as e:
print(e)
"""
self.thread_lock.release()
class StaticObjectDetectorNode:
def __init__(self):
self.name = 'static_object_detector_node'
self.tm = Matcher()
self.active = False
self.turn_counter = TurnCounter()
self.pub_ibvs = rospy.Publisher("~ibvs", Float32, queue_size=1)
self.sub_image = rospy.Subscriber("~image_compressed",
CompressedImage,
self.processImage,
buff_size=921600,
queue_size=1)
self.pub_image = rospy.Publisher("~servo_image", Image, queue_size=1)
self.pub_turns = rospy.Publisher("~turned", Bool, queue_size=1)
self.bridge = CvBridge()
rospy.loginfo("[%s] Initialized." % (self.name))
def cbSwitch(self, switch_msg):
self.active = switch_msg.data
def processImage(self, image_msg):
np_arr = np.fromstring(image_msg.data, np.uint8)
#image_cv=self.bridge.imgmsg_to_cv2(image_msg,"bgr8")
image_cv = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
img, center = self.tm.contour_match(image_cv)
crossing, turns = self.turn_counter.cbmsg(center)
if crossing:
# only trigger if it's been awhile
rospy.loginfo("Crossing. %d turn" % turns)
self.pub_turns.publish(Bool(data=True))
self.pub_ibvs.publish(Float32(data=center))
height, width = img.shape[:2]
"""
