class ObjectDetectionNode: def __init__(self): self.node_name = "Object Detection" self.active = True # Detector Configuration rospack = rospkg.RosPack() self.configuration = rospy.get_param( '~object_detector') # TODO: On-the-fly reconfiguration # TODO: Fix after a place for inference models is established self.object_detector = ObjectDetector( path.join(rospack.get_path('object_detection'), self.configuration['inference_graph_path']), float(self.configuration['score_threshold']), bool(self.configuration['denormalize_boundingbox'])) # Subscribers self.sub_image = rospy.Subscriber("~image_compressed", CompressedImage, self.on_image_received, queue_size=1) # Publishers self.pub_detection = rospy.Publisher("~detections", ObjectDetectionList, queue_size=1) # timer for updating the params # self.timer = rospy.Timer(rospy.Duration.from_sec(1.0), self.updateParams) def on_image_received(self, compressed_image): detections = self.object_detector.detect( rgb_from_ros(compressed_image)) if len(detections) > 0: detection_list_msg = ObjectDetectionList() for detection in detections: detection_list_msg.detections.append( Detection(class_label=detection['class_label'], class_id=detection['class_id'], xmin=detection['xmin'], xmax=detection['xmax'], ymin=detection['ymin'], ymax=detection['ymax'], score=detection['score'])) self.pub_detection.publish(detection_list_msg) def onShutdown(self): self.object_detector.finalize() rospy.loginfo("[ObjectDetectionNode] Shutdown.") def loginfo(self, s): rospy.loginfo('[%s] %s' % (self.node_name, s))
class ImageAIJob: def __init__(self): self.detector = ObjectDetector() # 'http://host.docker.internal:5000/api/' # self.base_url = os.environ["MEDIA_API_URL"] print("API Url: {}".format(self.base_url)) def run(self): r = requests.get('{}ai/MediaWithoutImageAISource'.format( self.base_url)) medias = r.json() for media in medias: id = media["id"] result = {} result["mediaId"] = id print("detect items for media: {}".format(id)) filename = "./tmp/{}.jpg".format(id) try: imageR = requests.get('{}ai/image/{}'.format( self.base_url, id), stream=True) imageR.raise_for_status() img = imageR.raw.read() with open(filename, 'wb') as f: f.write(img) items = self.detector.detect(filename, media["dimension"]) result["items"] = items result["success"] = True print("Found {} items in media: {}".format( len(result["items"]), id)) os.remove(filename) except Exception as e: result["success"] = False result["error"] = str(e) print(result["error"]) r_save = requests.post('{}ai/save'.format(self.base_url), json=result)
conf = Conf(args["conf"]) # load the classifier, then initialize the Histogram of Oriented Gradients descriptor # and the object detector model = pickle.loads(open(conf["classifier_path"], "rb").read()) hog = HOG(orientations=conf["orientations"], pixelsPerCell=tuple(conf["pixels_per_cell"]), cellsPerBlock=tuple(conf["cells_per_block"]), normalize=conf["normalize"], block_norm="L1") od = ObjectDetector(model, hog) # load the image and convert it to grayscale image = cv2.imread(args["image"]) image = imutils.resize(image, width=min(260, image.shape[1])) gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) # detect objects in the image (boxes, probs) = od.detect(gray, conf["window_dim"], winStep=conf["window_step"], pyramidScale=conf["pyramid_scale"], minProb=conf["min_probability"]) # loop over the bounding boxes and draw them for (startX, startY, endX, endY) in boxes: cv2.rectangle(image, (startX, startY), (endX, endY), (0, 0, 255), 2) # show the output images cv2.imshow("Image", image) cv2.waitKey(0)
class ObjectCategorizer(): def __init__(self): self.object_detector = ObjectDetector() # connect to collision map processing service rospy.loginfo('waiting for tabletop_collision_map_processing service') rospy.wait_for_service('/tabletop_collision_map_processing/tabletop_collision_map_processing') self.collision_map_processing_srv = rospy.ServiceProxy('/tabletop_collision_map_processing/tabletop_collision_map_processing', TabletopCollisionMapProcessing) rospy.loginfo('connected to tabletop_collision_map_processing service') # connect to gripper action server rospy.loginfo('waiting for wubble_gripper_grasp_action') self.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction) self.posture_controller.wait_for_server() rospy.loginfo('connected to wubble_gripper_grasp_action') rospy.loginfo('waiting for wubble_grasp_status service') rospy.wait_for_service('/wubble_grasp_status') self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) rospy.loginfo('connected to wubble_grasp_status service') rospy.loginfo('waiting for classify service') rospy.wait_for_service('/classify') self.classification_srv = rospy.ServiceProxy('/classify', classify) rospy.loginfo('connected to classify service') # connect to gripper action server rospy.loginfo('waiting for wubble_gripper_action') self.gripper_controller = SimpleActionClient('/wubble_gripper_action', WubbleGripperAction) self.gripper_controller.wait_for_server() rospy.loginfo('connected to wubble_gripper_action') # connect to wubble actions rospy.loginfo('waiting for drop_object') self.drop_object_client = SimpleActionClient('/drop_object', DropObjectAction) self.drop_object_client.wait_for_server() rospy.loginfo('connected to drop_object') rospy.loginfo('waiting for grasp_object') self.grasp_object_client = SimpleActionClient('/grasp_object', GraspObjectAction) self.grasp_object_client.wait_for_server() rospy.loginfo('connected to grasp_object') rospy.loginfo('waiting for lift_object') self.lift_object_client = SimpleActionClient('/lift_object', LiftObjectAction) self.lift_object_client.wait_for_server() rospy.loginfo('connected to lift_object') rospy.loginfo('waiting for place_object') self.place_object_client = SimpleActionClient('/place_object', PlaceObjectAction) self.place_object_client.wait_for_server() rospy.loginfo('connected to plcae_object') rospy.loginfo('waiting for push_object') self.push_object_client = SimpleActionClient('/push_object', PushObjectAction) self.push_object_client.wait_for_server() rospy.loginfo('connected to push_object') rospy.loginfo('waiting for shake_roll_object') self.shake_roll_object_client = SimpleActionClient('/shake_roll_object', ShakeRollObjectAction) self.shake_roll_object_client.wait_for_server() rospy.loginfo('connected to drop_object') rospy.loginfo('waiting for shake_pitch_object') self.shake_pitch_object_client = SimpleActionClient('/shake_pitch_object', ShakePitchObjectAction) self.shake_pitch_object_client.wait_for_server() rospy.loginfo('connected to pitch_object') rospy.loginfo('waiting for ready_arm') self.ready_arm_client = SimpleActionClient('/ready_arm', ReadyArmAction) self.ready_arm_client.wait_for_server() rospy.loginfo('connected to ready_arm') # advertise InfoMax service rospy.Service('get_category_distribution', InfoMax, self.process_infomax_request) rospy.loginfo('all services contacted, object_categorization is ready to go') self.ACTION_INFO = { InfomaxAction.GRASP: { 'client': self.grasp_object_client, 'goal': GraspObjectGoal(), 'prereqs': [InfomaxAction.GRASP] }, InfomaxAction.LIFT: { 'client': self.lift_object_client, 'goal': LiftObjectGoal(), 'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT] }, InfomaxAction.SHAKE_ROLL: { 'client': self.shake_roll_object_client, 'goal': ShakeRollObjectGoal(), 'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.SHAKE_ROLL] }, InfomaxAction.DROP: { 'client': self.drop_object_client, 'goal': DropObjectGoal(), 'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.DROP] }, InfomaxAction.PLACE: { 'client': self.place_object_client, 'goal': PlaceObjectGoal(), 'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.PLACE] }, InfomaxAction.PUSH: { 'client': self.push_object_client, 'goal': PushObjectGoal(), 'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.PLACE, InfomaxAction.PUSH] }, InfomaxAction.SHAKE_PITCH: { 'client': self.shake_pitch_object_client, 'goal': ShakePitchObjectGoal(), 'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.SHAKE_PITCH] }, } def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() def gentle_close_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.GRASP self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() goal = WubbleGripperGoal() goal.command = WubbleGripperGoal.CLOSE_GRIPPER goal.torque_limit = 0.0 goal.dynamic_torque_control = False self.gripper_controller.send_goal(goal) self.gripper_controller.wait_for_result() def segment_objects(self): res = self.object_detector.detect() if res is None: rospy.logerr('TabletopSegmentation did not find any clusters') return None segmentation_result = res['segmentation_result'] self.info = res['cluster_information'] tdr = TabletopDetectionResult() tdr.table = segmentation_result.table tdr.clusters = segmentation_result.clusters tdr.result = segmentation_result.result tcmpr = self.update_collision_map(tdr) return tcmpr def reset_collision_map(self): req = TabletopCollisionMapProcessingRequest() req.detection_result = TabletopDetectionResult() req.reset_collision_models = True req.reset_attached_models = True self.collision_map_processing_srv(req) rospy.loginfo('collision map reset') def update_collision_map(self, tabletop_detection_result): rospy.loginfo('collision_map update in progress') req = TabletopCollisionMapProcessingRequest() req.detection_result = tabletop_detection_result req.reset_collision_models = True req.reset_attached_models = True req.desired_frame = 'base_link' res = self.collision_map_processing_srv(req) rospy.loginfo('collision_map update is done') rospy.loginfo('there are %d graspable objects %s, collision support surface name is "%s"' % (len(res.graspable_objects), res.collision_object_names, res.collision_support_surface_name)) return res def reset_robot(self, tabletop_collision_map_processing_result=None): rospy.loginfo('resetting robot') ordered_collision_operations = OrderedCollisionOperations() if tabletop_collision_map_processing_result: closest_index = self.info[0][0] collision_object_name = tabletop_collision_map_processing_result.collision_object_names[closest_index] collision_support_surface_name = tabletop_collision_map_processing_result.collision_support_surface_name collision_operation = CollisionOperation() collision_operation.object1 = collision_support_surface_name collision_operation.object2 = ARM_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE collision_operation.penetration_distance = 0.02 ordered_collision_operations.collision_operations = [collision_operation] collision_operation = CollisionOperation() collision_operation.object1 = collision_support_surface_name collision_operation.object2 = GRIPPER_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE collision_operation.penetration_distance = 0.02 ordered_collision_operations.collision_operations.append(collision_operation) collision_operation = CollisionOperation() collision_operation.object1 = collision_object_name collision_operation.object2 = GRIPPER_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE collision_operation.penetration_distance = 0.02 ordered_collision_operations.collision_operations.append(collision_operation) else: self.reset_collision_map() current_state = find_current_arm_state() rospy.loginfo('arm is currently in %s state' % current_state) if current_state not in ['READY', 'TUCK']: # check if the gripper is empty grasp_status = self.get_grasp_status_srv() if grasp_status.is_hand_occupied: rospy.loginfo('arm is not in any of the following states %s, opening gripper' % str(['READY', 'TUCK'])) self.open_gripper() if current_state != 'READY' and not self.ready_arm(ordered_collision_operations): return False self.gentle_close_gripper() return True def ready_arm(self, collision_operations=OrderedCollisionOperations()): """ Moves the arm to the side out of the view of all sensors. In this position the arm is ready to perform a grasp action. """ goal = ReadyArmGoal() goal.collision_operations = collision_operations state = self.ready_arm_client.send_goal_and_wait(goal) if state == GoalStatus.SUCCEEDED: return True else: return False def execute_action(self, action, tabletop_collision_map_processing_result): goal = self.ACTION_INFO[action]['goal'] goal.category_id = self.category_id closest_index = self.info[0][0] goal.graspable_object = tabletop_collision_map_processing_result.graspable_objects[closest_index] goal.collision_object_name = tabletop_collision_map_processing_result.collision_object_names[closest_index] goal.collision_support_surface_name = tabletop_collision_map_processing_result.collision_support_surface_name state = self.ACTION_INFO[action]['client'].send_goal_and_wait(goal) if state == GoalStatus.SUCCEEDED: result = self.ACTION_INFO[action]['client'].get_result() return result.recorded_sound else: return None # receive an InfoMax service request containing object ID and desired action def process_infomax_request(self, req): self.object_names = req.objectNames self.action_names = req.actionNames self.num_categories = req.numCats self.category_id = req.catID if not self.reset_robot(): return None # find a graspable object on the floor tcmpr = self.segment_objects() if tcmpr is None: return None # initialize as uniform distribution beliefs = [1.0/self.num_categories] * self.num_categories actions = self.ACTION_INFO[req.actionID.val]['prereqs'] for act in actions: sound = self.execute_action(act, tcmpr) if not sound: self.reset_robot(tcmpr) return None else: resp = self.classification_srv(sound) beliefs = resp.beliefs if not self.reset_robot(tcmpr): return None res = InfoMaxResponse() res.beliefs = beliefs res.location = self.category_id return res
class ObjectCategorizer(): def __init__(self): self.object_detector = ObjectDetector() # connect to collision map processing service rospy.loginfo('waiting for tabletop_collision_map_processing service') rospy.wait_for_service( '/tabletop_collision_map_processing/tabletop_collision_map_processing' ) self.collision_map_processing_srv = rospy.ServiceProxy( '/tabletop_collision_map_processing/tabletop_collision_map_processing', TabletopCollisionMapProcessing) rospy.loginfo('connected to tabletop_collision_map_processing service') # connect to gripper action server rospy.loginfo('waiting for wubble_gripper_grasp_action') self.posture_controller = SimpleActionClient( '/wubble_gripper_grasp_action', GraspHandPostureExecutionAction) self.posture_controller.wait_for_server() rospy.loginfo('connected to wubble_gripper_grasp_action') rospy.loginfo('waiting for wubble_grasp_status service') rospy.wait_for_service('/wubble_grasp_status') self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) rospy.loginfo('connected to wubble_grasp_status service') rospy.loginfo('waiting for classify service') rospy.wait_for_service('/classify') self.classification_srv = rospy.ServiceProxy('/classify', classify) rospy.loginfo('connected to classify service') # connect to gripper action server rospy.loginfo('waiting for wubble_gripper_action') self.gripper_controller = SimpleActionClient('/wubble_gripper_action', WubbleGripperAction) self.gripper_controller.wait_for_server() rospy.loginfo('connected to wubble_gripper_action') # connect to wubble actions rospy.loginfo('waiting for drop_object') self.drop_object_client = SimpleActionClient('/drop_object', DropObjectAction) self.drop_object_client.wait_for_server() rospy.loginfo('connected to drop_object') rospy.loginfo('waiting for grasp_object') self.grasp_object_client = SimpleActionClient('/grasp_object', GraspObjectAction) self.grasp_object_client.wait_for_server() rospy.loginfo('connected to grasp_object') rospy.loginfo('waiting for lift_object') self.lift_object_client = SimpleActionClient('/lift_object', LiftObjectAction) self.lift_object_client.wait_for_server() rospy.loginfo('connected to lift_object') rospy.loginfo('waiting for place_object') self.place_object_client = SimpleActionClient('/place_object', PlaceObjectAction) self.place_object_client.wait_for_server() rospy.loginfo('connected to plcae_object') rospy.loginfo('waiting for push_object') self.push_object_client = SimpleActionClient('/push_object', PushObjectAction) self.push_object_client.wait_for_server() rospy.loginfo('connected to push_object') rospy.loginfo('waiting for shake_roll_object') self.shake_roll_object_client = SimpleActionClient( '/shake_roll_object', ShakeRollObjectAction) self.shake_roll_object_client.wait_for_server() rospy.loginfo('connected to drop_object') rospy.loginfo('waiting for shake_pitch_object') self.shake_pitch_object_client = SimpleActionClient( '/shake_pitch_object', ShakePitchObjectAction) self.shake_pitch_object_client.wait_for_server() rospy.loginfo('connected to pitch_object') rospy.loginfo('waiting for ready_arm') self.ready_arm_client = SimpleActionClient('/ready_arm', ReadyArmAction) self.ready_arm_client.wait_for_server() rospy.loginfo('connected to ready_arm') # advertise InfoMax service rospy.Service('get_category_distribution', InfoMax, self.process_infomax_request) rospy.loginfo( 'all services contacted, object_categorization is ready to go') self.ACTION_INFO = { InfomaxAction.GRASP: { 'client': self.grasp_object_client, 'goal': GraspObjectGoal(), 'prereqs': [InfomaxAction.GRASP] }, InfomaxAction.LIFT: { 'client': self.lift_object_client, 'goal': LiftObjectGoal(), 'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT] }, InfomaxAction.SHAKE_ROLL: { 'client': self.shake_roll_object_client, 'goal': ShakeRollObjectGoal(), 'prereqs': [ InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.SHAKE_ROLL ] }, InfomaxAction.DROP: { 'client': self.drop_object_client, 'goal': DropObjectGoal(), 'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.DROP] }, InfomaxAction.PLACE: { 'client': self.place_object_client, 'goal': PlaceObjectGoal(), 'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.PLACE] }, InfomaxAction.PUSH: { 'client': self.push_object_client, 'goal': PushObjectGoal(), 'prereqs': [ InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.PLACE, InfomaxAction.PUSH ] }, InfomaxAction.SHAKE_PITCH: { 'client': self.shake_pitch_object_client, 'goal': ShakePitchObjectGoal(), 'prereqs': [ InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.SHAKE_PITCH ] }, } def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() def gentle_close_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.GRASP self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() goal = WubbleGripperGoal() goal.command = WubbleGripperGoal.CLOSE_GRIPPER goal.torque_limit = 0.0 goal.dynamic_torque_control = False self.gripper_controller.send_goal(goal) self.gripper_controller.wait_for_result() def segment_objects(self): res = self.object_detector.detect() if res is None: rospy.logerr('TabletopSegmentation did not find any clusters') return None segmentation_result = res['segmentation_result'] self.info = res['cluster_information'] tdr = TabletopDetectionResult() tdr.table = segmentation_result.table tdr.clusters = segmentation_result.clusters tdr.result = segmentation_result.result tcmpr = self.update_collision_map(tdr) return tcmpr def reset_collision_map(self): req = TabletopCollisionMapProcessingRequest() req.detection_result = TabletopDetectionResult() req.reset_collision_models = True req.reset_attached_models = True self.collision_map_processing_srv(req) rospy.loginfo('collision map reset') def update_collision_map(self, tabletop_detection_result): rospy.loginfo('collision_map update in progress') req = TabletopCollisionMapProcessingRequest() req.detection_result = tabletop_detection_result req.reset_collision_models = True req.reset_attached_models = True req.desired_frame = 'base_link' res = self.collision_map_processing_srv(req) rospy.loginfo('collision_map update is done') rospy.loginfo( 'there are %d graspable objects %s, collision support surface name is "%s"' % (len(res.graspable_objects), res.collision_object_names, res.collision_support_surface_name)) return res def reset_robot(self, tabletop_collision_map_processing_result=None): rospy.loginfo('resetting robot') ordered_collision_operations = OrderedCollisionOperations() if tabletop_collision_map_processing_result: closest_index = self.info[0][0] collision_object_name = tabletop_collision_map_processing_result.collision_object_names[ closest_index] collision_support_surface_name = tabletop_collision_map_processing_result.collision_support_surface_name collision_operation = CollisionOperation() collision_operation.object1 = collision_support_surface_name collision_operation.object2 = ARM_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE collision_operation.penetration_distance = 0.02 ordered_collision_operations.collision_operations = [ collision_operation ] collision_operation = CollisionOperation() collision_operation.object1 = collision_support_surface_name collision_operation.object2 = GRIPPER_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE collision_operation.penetration_distance = 0.02 ordered_collision_operations.collision_operations.append( collision_operation) collision_operation = CollisionOperation() collision_operation.object1 = collision_object_name collision_operation.object2 = GRIPPER_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE collision_operation.penetration_distance = 0.02 ordered_collision_operations.collision_operations.append( collision_operation) else: self.reset_collision_map() current_state = find_current_arm_state() rospy.loginfo('arm is currently in %s state' % current_state) if current_state not in ['READY', 'TUCK']: # check if the gripper is empty grasp_status = self.get_grasp_status_srv() if grasp_status.is_hand_occupied: rospy.loginfo( 'arm is not in any of the following states %s, opening gripper' % str(['READY', 'TUCK'])) self.open_gripper() if current_state != 'READY' and not self.ready_arm( ordered_collision_operations): return False self.gentle_close_gripper() return True def ready_arm(self, collision_operations=OrderedCollisionOperations()): """ Moves the arm to the side out of the view of all sensors. In this position the arm is ready to perform a grasp action. """ goal = ReadyArmGoal() goal.collision_operations = collision_operations state = self.ready_arm_client.send_goal_and_wait(goal) if state == GoalStatus.SUCCEEDED: return True else: return False def execute_action(self, action, tabletop_collision_map_processing_result): goal = self.ACTION_INFO[action]['goal'] goal.category_id = self.category_id closest_index = self.info[0][0] goal.graspable_object = tabletop_collision_map_processing_result.graspable_objects[ closest_index] goal.collision_object_name = tabletop_collision_map_processing_result.collision_object_names[ closest_index] goal.collision_support_surface_name = tabletop_collision_map_processing_result.collision_support_surface_name state = self.ACTION_INFO[action]['client'].send_goal_and_wait(goal) if state == GoalStatus.SUCCEEDED: result = self.ACTION_INFO[action]['client'].get_result() return result.recorded_sound else: return None # receive an InfoMax service request containing object ID and desired action def process_infomax_request(self, req): self.object_names = req.objectNames self.action_names = req.actionNames self.num_categories = req.numCats self.category_id = req.catID if not self.reset_robot(): return None # find a graspable object on the floor tcmpr = self.segment_objects() if tcmpr is None: return None # initialize as uniform distribution beliefs = [1.0 / self.num_categories] * self.num_categories actions = self.ACTION_INFO[req.actionID.val]['prereqs'] for act in actions: sound = self.execute_action(act, tcmpr) if not sound: self.reset_robot(tcmpr) return None else: resp = self.classification_srv(sound) beliefs = resp.beliefs if not self.reset_robot(tcmpr): return None res = InfoMaxResponse() res.beliefs = beliefs res.location = self.category_id return res