def countPeople(self): # Get the current table from the parameter server current_table = rospy.get_param('/current_table') object_count = defaultdict(int) # Get Cuboid for Min and Max points of the table cuboid = rospy.get_param('/tables/' + current_table + '/cuboid') tables = rospy.get_param('/tables') locations = tables[current_table]['locations'] rospy.loginfo('locations {}'.format(locations)) # Get Left and Right points of the sides of the table side_points = rospy.get_param('/tables/' + current_table + '/lookLR') # Take a picture using the depth mask and feed it to the detection for i in range(len(side_points)): self.lookAt(side_points[i]) rospy.loginfo('Getting the image..') depth_points, image = self.getPcl2AndImage() rospy.loginfo('GOT THE IMAGE GOT EEEM ') mask_msg = self.getDepthMask(depth_points, cuboid['min_xyz'], cuboid['max_xyz']) rospy.loginfo('FINISHED FROM DE MASK BRODA') image_masked = self.applyDepthMask(image, mask_msg.mask, 175) count_objects_result = self.detectObject(image_masked, "coco", 0.5, 0.3) # update dictionary for detection in count_objects_result.detected_objects: object_count[detection.name] += 1 bridge = CvBridge() frame = bridge.imgmsg_to_cv2(count_objects_result.image_bb, "bgr8") # Save img to img dir for logging rospack = rospkg.RosPack() savedir = rospack.get_path('lasr_sciroc') + '/images/' now = datetime.now() cv2.imwrite(savedir + now.strftime("%Y-%m-%d-%H:%M:%S") + '.png', frame) # RETURN TO DEFAULT POSE self.playMotion('back_to_default') # calculate and output result person_count = object_count['person'] speech_out = 'I see ' + str(person_count) + ' person' if not person_count == 1: speech_out += 's' self.talk(speech_out) # Update the number of people in the parameter server rospy.loginfo('Updating the number of people found at %s' % current_table) rospy.set_param('/tables/' + current_table + '/person_count', person_count) rospy.loginfo('Updated the person counter successfully') # TODO: Make an updateHubTableStatus function that does the below since this block of code is duplicated a lot # MKHub bridge object bridge = MKHubBridge('https://api.mksmart.org/sciroc-competition', 'leedsasr', 'sciroc-episode3-table') # Post the people count of the table to the data hub status = rospy.get_param('/tables/' + current_table + '/status') payload = bridge.constructTablePayload(current_table, person_count, status) print('PRINTING PAYLOAD') print(payload) response = bridge.post(current_table, payload)
def identifyStatus(self): current_table = rospy.get_param('/current_table') rospy.loginfo('Identifying the status of: %s' % current_table) # Step 1: Look down to see the table self.playMotion('check_table') # Step 2: YOLOv3 object detection cuboid = rospy.get_param('/tables/' + current_table + '/cuboid') depth_points, image = self.getPcl2AndImage() mask_msg = self.getDepthNanMask(depth_points, cuboid['min_xyz'], cuboid['max_xyz']) image_masked = self.applyDepthMask(image, mask_msg.mask, 175) count_objects_result = self.detectObject(image_masked, "costa", 0.7, 0.3) bridge = CvBridge() frame = bridge.imgmsg_to_cv2(count_objects_result.image_bb, "bgr8") # Save img to img dir for logging rospack = rospkg.RosPack() savedir = rospack.get_path('lasr_sciroc') + '/images/' now = datetime.now() cv2.imwrite(savedir + now.strftime("%Y-%m-%d-%H:%M:%S") + '.png', frame) # dictionary of results object_count = defaultdict(int) for detection in count_objects_result.detected_objects: object_count[detection.name] += 1 # output results if len(object_count): speech_out = 'I see ' for costa_object in object_count: speech_out += ', ' + str( object_count[costa_object]) + ' ' + str(costa_object) if not object_count[costa_object] == 1: speech_out += 's' self.talk(speech_out) else: self.talk('no objects found') # Step 3: Get head and torso back to default self.playMotion('back_to_default') # Step 4: Decide on table status and send tts goal to the sound server foundPerson = rospy.get_param('/tables/' + current_table + '/person_count') foundConsumable = len(object_count) if foundPerson and foundConsumable: status = 'Already served' elif foundPerson and not foundConsumable: status = 'Needs serving' elif not foundPerson and foundConsumable: status = 'Needs cleaning' else: status = 'Ready' # Update the status of the table in the parameter server rospy.loginfo('Updating the status of %s', current_table) rospy.set_param('/tables/' + current_table + '/status', status) rospy.loginfo('Updated the status successfully') # output status self.talk('Status of {0} is {1}'.format(current_table, status)) # MKHub bridge object bridge = MKHubBridge('https://api.mksmart.org/sciroc-competition', 'leedsasr', 'sciroc-episode3-table') # Post the status of the table to the data hub person_count = rospy.get_param('/tables/' + current_table + '/person_count') payload = bridge.constructTablePayload(current_table, person_count, status) print('PRINTING PAYLOAD') print(payload) response = bridge.post(current_table, payload)