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] """