def evaluate(self) -> None: db_api = DatabaseAPI() demonstration_templates = db_api.get_demonstration_templates() uim = UserInputManager() uim.say("Actions learned from demonstration:") uim.show([t.name for t in demonstration_templates])
def __init__(self): rospy.init_node('execute_program_node', anonymous=False) logging.config.fileConfig(get_full_path('config', 'logging.ini')) self._getdatsrv = rospy.ServiceProxy('/db_interface_node/getDatabase', GetDatabase) req = GetDatabaseRequest() req.write = False res = self._getdatsrv.call(req) # self.db = Database(res.path) self._addsrv = rospy.ServiceProxy('/db_interface_node/addObject', AddObject) self._senddatsrv = rospy.ServiceProxy('/db_interface_node/sendDatabase', SendDatabase) self.db_api = DatabaseAPI() # self.onto = self.db_api.get_onto() # path = '/home/algernon/ros_melodic_ws/base_ws/src/crow/database_com/nodes/saved_onto.owl' self.db = self.db_api.get_db() self.db.change_onto(res.path) obj = self.db.onto.search(type=self.db.onto.Cube) # exit_res = self.db.onto.__exit__() print('press a key to execute an action') self.robotProgram = 'grounded_1' self.listener = keyboard.Listener(on_press=self.on_press) self.listener.start()
def __init__(self, language = 'en'): self.lang = language self.db_api = DatabaseAPI() self.cd = ColorDetector() self.ar = UserInputManager(language = self.lang) self.logger = logging.getLogger(__name__) self.templ_file = self.ar.load_file('templates_detection.json') self.guidance_file = self.ar.load_file('guidance_dialogue.json')
def evaluate(self) -> None: is_default = False uim = UserInputManager() uim.say(f"Do you want to make {self.area_name} your default area? (y/n)") if uim.confirm_user(): is_default = True self.db_api = DatabaseAPI() self.db_api.add_area(area_name=self.area_name, corners=(self.corner_0, self.corner_1, self.corner_2, self.corner_3), is_default=is_default) self.db_api.set_state(State.DEFINE_AREA)
def __init__(self, language='en'): self.logger = logging.getLogger(__name__) self.og = ObjectGrounder() self.lg = LocationGrounder() self.db_api = DatabaseAPI() self.lang = language
class DefineAreaAction(db.onto.Template): namespace = db.onto def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) self.register_parameter(name="area_name", value=str) self.register_parameter(name="corner_0", value=db.onto.Location) self.register_parameter(name="corner_1", value=db.onto.Location) self.register_parameter(name="corner_2", value=db.onto.Location) self.register_parameter(name="corner_3", value=db.onto.Location) def match(self, tagged_text : TaggedText) -> None: # match text until "with" or "corners" is encountered match = tagged_text.match_regex(r"called (.*?)(?= with| corners)") if match: self.area_name = match.group(1) else: uim = UserInputManager() self.area_name = uim.ask_for_name("area") # match eight digits for area corners regex_pos_list = [("corners", "NN")] + ([(r".*", "CD")] * 8) matches = tagged_text.match_regex_pos_list(regex_pos_list) for i in range(1, len(matches), 2): coord_x = nlp.get_int(matches[i].string) coord_y = nlp.get_int(matches[i+1].string) corner = db.onto.Location(x=coord_x, y=coord_y, z=0) setattr(self, f"corner_{i // 2}", corner) def evaluate(self) -> None: is_default = False uim = UserInputManager() uim.say(f"Do you want to make {self.area_name} your default area? (y/n)") if uim.confirm_user(): is_default = True self.db_api = DatabaseAPI() self.db_api.add_area(area_name=self.area_name, corners=(self.corner_0, self.corner_1, self.corner_2, self.corner_3), is_default=is_default) self.db_api.set_state(State.DEFINE_AREA)
def add_put_first_cube_task(self, program, cube_id): put_task = PutTask() top_cube = db.onto.ObjectPlaceholder() top_cube.is_a.append(db.onto.Cube) top_cube.id = cube_id put_task.object_to_put = top_cube db_api = DatabaseAPI() default_area = db_api.get_default_area() put_task.location = RelativeLocation() put_task.location.loc_type = "center" put_task.location.relative_to = default_area self.add_task(program, put_task)
def evaluate(self) -> None: self.db_api = DatabaseAPI() self.db_api.set_param("current_task_name", self.task_name) dl = DemonstrationLearner() program = dl.create_new_program() last_cube_positions = {} for cube_positions in dl.get_cube_positions(): for key, val in cube_positions.items(): val self.add_object('Cube', x=0.1, y=0.3, z=0, id=key, color='red') logging.debug(f"Position of cubes: {cube_positions}") overlapping = self.detect_overlapping(last_cube_positions, cube_positions) if overlapping: bottom_cube_id = overlapping[0] top_cube_id = overlapping[1] logging.debug( f"Overlapping cubes detected: cube id {top_cube_id} is on top of cube id {bottom_cube_id}" ) if not program.root.children: # beginning of the demonstration, place the first cube self.add_pick_task(program, bottom_cube_id) self.add_put_first_cube_task(program, bottom_cube_id) self.add_pick_task(program, top_cube_id) self.add_put_task(program, top_cube_id, bottom_cube_id) last_cube_positions = cube_positions # time.sleep(1) logging.debug("Demonstration finished, adding program to database") logging.debug("\n" + str(program)) self.db_api.add_custom_template(program) self.db_api.set_state(State.LEARN_FROM_DEMONSTRATION)
def __init__(self): self.logger = logging.getLogger(__name__) self.db_api = DatabaseAPI() self.class_map = { "screwdriver": db.onto.Screwdriver, "hammer": db.onto.Hammer, "pliers": db.onto.Pliers, "glue": db.onto.Glue, "panel": db.onto.Panel, "cube": db.onto.Cube }
def __init__(self): rospy.init_node('process_sentence_node', anonymous=False) logging.config.fileConfig(get_full_path('config', 'logging.ini')) self._getdatsrv = rospy.ServiceProxy('/db_interface_node/getDatabase', GetDatabase) req = GetDatabaseRequest() req.write = False res = self._getdatsrv.call(req) self.sub = rospy.Subscriber('/nl_input', SentenceProgram, queue_size=10, callback=self.callback) self.pub = rospy.Publisher('/sentence_output', SentenceProgram, queue_size=10) self._addsrv = rospy.ServiceProxy('/db_interface_node/addObject', AddObject) self._senddatsrv = rospy.ServiceProxy( '/db_interface_node/sendDatabase', SendDatabase) self.db_api = DatabaseAPI() self.db = self.db_api.get_db() self.ontoC = self.db.change_onto(res.path) #obj = self.db.onto.search(type = db.onto.Cube) #obj2 = self.ontoC.search(type = db.onto.Cube) #obj3 = self.db.onto.search(type = db.onto.Cube) #obj4 = self.ontoC.search(type = db.onto.Cube) #path = '/home/algernon/ros_melodic_ws/base_ws/src/crow_nlp/scripts/saved_updated_onto.owl' self.UngroundedID = 1 self.GroundedID = 1
def process_text(self, sentence: str) -> RobotProgram: """ Turns an input text into a program template which can be used for creating instructions for the robot (after grounding). The program template is dependent only on the content of the sentence and not on the current state of the workspace. Parameters ---------- sentence an input sentence as string Returns ------- a program template - formalized instructions for the robot with placeholders for real objects and locations (right now, the behavior is undefined in case the sentence does not allow creating a valid program) """ parsed_text = self.gp.parse(sentence) root = parsed_text.parse_tree db_api = DatabaseAPI() state = db_api.get_state() if state == State.LEARN_FROM_INSTRUCTIONS: program = RobotCustomProgram() else: program = RobotProgram() # hardcoded program structure: all subprograms are located directly under the root node "AND" program.root = RobotProgramOperator(operator_type="AND") for subnode in root.subnodes: if type(subnode) is ParseTreeNode: # create a single robot instructon program_node = self.process_node(subnode) program.root.add_child(program_node) return program
class ObjectGrounder: namespace = db.onto #class Flags(Enum): # CAN_BE_PICKED = 1 def __init__(self): self.db_api = DatabaseAPI() self.cd = ColorDetector() self.ar = UserInputManager() self.logger = logging.getLogger(__name__) def ground_object(self, obj_placeholder, flags=()) -> Any: # try to find out the class of the object # the 0th item of an is_a list should be always ObjectPlaceholder # if the item is bound to a real class, the class will be the 1st item cls = obj_placeholder.is_a[-1] if "last_mentioned" in obj_placeholder.flags: objs = [self.db_api.get_last_mentioned_object()] else: props = obj_placeholder.get_properties() props.discard(db.onto.hasFlags) # this is internal property of the object placeholder # put together a dictionary of properties required from the object props_vals = {self.get_prop_name(prop): getattr(obj_placeholder, self.get_prop_name(prop)) for prop in props} # find if there is an object with such properties in the workspace objs = self.db_api.get_by_properties(cls=cls, properties=props_vals) # if self.Flags.CAN_BE_PICKED in flags: # objs = list(filter(lambda x: x._can_be_picked, objs)) # only one object found / any object can be selected if len(objs) == 1 or ("any" in obj_placeholder.flags and len(objs) > 0): obj = objs[0] self.db_api.set_last_mentioned_object(obj) # more objects -> ask the user to select one elif len(objs) > 1: obj = self.ar.ask_to_select_from_class_objects(objs) self.db_api.set_last_mentioned_object(obj) else: self.logger.warning(f"No object of type {cls} in the workspace.") obj = None if obj: self.logger.debug(f"Object found for {obj_placeholder}: {obj}") return obj def get_prop_name(self, prop : ow.DataPropertyClass): # we need to use the python name of properties whenever it is defined if hasattr(prop, "python_name"): return prop.python_name return prop.name
def __init__(self): rospy.init_node('db_interface_node', anonymous=False) self._dbapi = DatabaseAPI() self.db = self._dbapi.get_db() self.sub_cubes = rospy.Subscriber('/averaged_markers', MarkersAvgList, self.update_object_position) # self.listener = keyboard.Listener(on_press=self.on_press) # self.listener.start() self.pub = rospy.Publisher('/database_update', DatabaseUpdate, queue_size=10) self.srv = {} # self.srv['groundProgram'] = rospy.Service('~groundProgram', GroundProgram, self.handle_ground_program) self.srv['addObject'] = rospy.Service('~addObject', AddObject, self.handle_add_object) self.srv['getDatabase'] = rospy.Service( '~getDatabase', GetDatabase, self.handle_get_database_request) self.srv['sendDatabase'] = rospy.Service( '~sendDatabase', SendDatabase, self.handle_send_database_request) self.database_locked = False
def __init__(self,language = 'en'): self.logger = logging.getLogger(__name__) self.lang = language self.db_api = DatabaseAPI() self.ui = UserInputManager(language = language) self.class_map = { "screwdriver": db.onto.Screwdriver, "hammer": db.onto.Hammer, "pliers": db.onto.Pliers, "glue" : db.onto.Glue, "panel" : db.onto.Panel, "cube" : db.onto.Cube } self.templ_det = self.ui.load_file('templates_detection.json') self.guidance_file = self.ui.load_file('guidance_dialogue.json')
class AreaDetector(CrowModule): namespace = db.onto def __init__(self): self.logger = logging.getLogger(__name__) self.db_api = DatabaseAPI() def detect_area(self, tagged_text: TaggedText) -> db.onto.ObjectPlaceholder: area_list = self.db_api.get_class_objects(db.onto.Area) for area in area_list: if tagged_text.contains_text(area.name): self.logger.debug( f"Area detected for \"{tagged_text.get_text()}\": {area}") return area return None
class SpeechProcessor(): WRONG_INPUT_STATE = 0 SEND_ERRORS = False # whether to send error to OPC server or just show them in console USE_ACTIVE_STATE = False STRIP_ACCENTS = True # whether to remove accents before comparing recognized text and possible choices ALLOW_INTERRUPTIONS = False # whether the user can interupt TTS playback MICROPHONE_WAIT_TIMEOUT = 5 # time to wait for any non-zero audio from mic LISTENING_START_TIMEOUT = 5 # time to start speaking after recognition is ran PHRASE_TIMEOUT = 4 # maximum length of phrase before listening is cut off CALIBRATION_TIME = 1 # time to spend calibrating the microphone bgr energy levels def __init__(self, serverAdress, gain: int = 1.0): # instantiate client object; make sure address and port are correct self.DEBUG_mode = False self.client = Client(serverAdress) self.shouldProcessInput = True self.LANG = 'en' self.recognizer = sr.Recognizer() self.fast_recognizer = sr.Recognizer() self.fast_recognizer.non_speaking_duration = 0.05 self.fast_recognizer.pause_threshold = 0.15 self.recText = RawNLParser(language=self.LANG) self.lastRecognitionFailed = False self.repeatChoices = False self.last_key = '' self.keys_pressed = set() if self.DEBUG_mode: self.keys_pressed = 's' self.ADD_OBJECTS = 1 self.kb_listener = Listener(on_press=self.on_press) self.kb_listener.start() self.play_sound("start_speech") """ SoX Effects http://sox.sourceforge.net/sox.html#EFFECTS gain -n normalize the audio to 0dB -b balance the audio, try to prevent clipping dither adds noise to mask low sampling rate vol changes volume: above 1 -> increase volume below 1 -> decrease volume pad adds silence to the begining (first argument) and the end (second argument) of the audio - attempt to mask sound cut-off but extends the audio duration """ self.sox_effects = ("gain", "-n", "-b", "vol", str(gain), "pad", "0", "0.5", "dither", "-a") SpeechCustom.MAX_SEGMENT_SIZE = 300 Speech.MAX_SEGMENT_SIZE = 300 self.hint_directive_re = re.compile(r"<([b-])>\s*$") rospy.init_node('process_sentence_node', anonymous=False) logging.config.fileConfig(get_full_path('config', 'logging.ini')) self._getdatsrv = rospy.ServiceProxy('/db_interface_node/getDatabase', GetDatabase) req = GetDatabaseRequest() req.write = False res = self._getdatsrv.call(req) self.sub = rospy.Subscriber('/nl_input', SentenceProgram, queue_size=10, callback=self.callback) self.pub = rospy.Publisher('/sentence_output', SentenceProgram, queue_size=10) self._addsrv = rospy.ServiceProxy('/db_interface_node/addObject', AddObject) self._senddatsrv = rospy.ServiceProxy( '/db_interface_node/sendDatabase', SendDatabase) self.db_api = DatabaseAPI() self.db = self.db_api.get_db() self.ontoC = self.db.change_onto(res.path) # obj = self.db.onto.search(type = db.onto.Cube) # obj2 = self.ontoC.search(type = db.onto.Cube) # obj3 = self.db.onto.search(type = db.onto.Cube) # obj4 = self.ontoC.search(type = db.onto.Cube) # path = '/home/algernon/ros_melodic_ws/base_ws/src/crow_nlp/scripts/saved_updated_onto.owl' self.UngroundedID = 1 self.GroundedID = 1 def on_press(self, key): self.last_key = key if isinstance(key, KeyCode): self.keys_pressed.add(key.char) print('{0} pressed'.format(key)) def connect(self): self.client.connect() # connect to server root = self.client.nodes.root # get the root entity print("Connected to server and successfully retrieved the root.") # Retreive some objects and variables dataObj = root.get_child(["0:Objects", "4:DATA"]) # a horse horse = dataObj.get_child("4:horse") # mic_active is the "You can now try recognizing speech" variable self.mic_active = horse.get_child("4:request") # the string where responses (errors and such) should be sent self.response_string = horse.get_child("4:response") # the state change request variable self.request = horse.get_child(["4:next_state_choice", "4:request"]) # self.request.set_value(True) # the number of the state to be changed to self.request_state_num = horse.get_child( ["4:next_state_choice", "4:state_num"]) # the variable with next state choices self.next_state_possibilities = horse.get_child( "4:next_state_possibilities") # self.client.load_type_definitions() # the variable with the current state self.actual_state_number = horse.get_child( ["4:actual_state", "4:number"]) self.sub = self.client.create_subscription(100, self) # create subscription self.handle = self.sub.subscribe_data_change(self.mic_active) def _send_state(self, state_num): """ Changes state to the specified number and sets "request" to True. """ dt = ua.DataValue(ua.Variant(state_num, ua.VariantType.Int16)) self.request_state_num.set_value(dt) self.request.set_value(True) def __extract_directive(self, string): result = self.hint_directive_re.split(string) if len(result) > 1: return result[0], result[1] else: return result[0], "" def init(self, configure_deivce=False): self.load_files() print("Audio input devices on this system:\n\tindex\tname") default_device_idx = sd.default.device[0] #default_device_idx = 12 device_list = sr.Microphone.list_microphone_names() for index, name in enumerate(device_list): if index == default_device_idx: d = ">\t" add = "(CURRENT)" else: d = "\t" add = "" print(f"{d}{index}:\t\t{name} {add}") self.microphone_index = 12 #default_device_idx if type(configure_deivce) is bool and configure_deivce: if INPUTIMEOUT_IMPORT_SUCCESS: timout = 10 try: self.microphone_index = int( inputimeout( prompt= f"Select device to use within {timout} seconds (enter to select the default device)\nDefault device '{default_device_idx}: {device_list[default_device_idx]}': ", timeout=timout) or default_device_idx) except TimeoutOccurred: print("Selecting the default device.") else: self.microphone_index = int( input( prompt= f"Select device to use (enter to select the default device)\nDefault device '{default_device_idx}: {device_list[default_device_idx]}': " ) or default_device_idx) if self.microphone_index != default_device_idx: print( f"Selected device with index {self.microphone_index}: {device_list[self.microphone_index]}." ) else: print("Selected the default device.") elif type(configure_deivce) is int: self.microphone_index = configure_deivce print( f"Selected device with index {self.microphone_index}: {device_list[self.microphone_index]} from command line." ) self.stream_device = [self.microphone_index, sd.default.device[1]] print( "Calibrating microphone. Make sure it is turned on and that no one is speaking." ) self._notbreak = True with sr.Microphone(device_index=self.microphone_index) as source: # wait for a second to let the recognizer adjust the # energy threshold based on the surrounding noise level self.recognizer.adjust_for_ambient_noise( source, duration=self.CALIBRATION_TIME) self.fast_recognizer.energy_threshold = self.recognizer.energy_threshold # self.recognizer.dynamic_energy_threshold = True print("Calibration done.") self.print_message("Ready.") def say(self, req, say=True, screen=True): """ produce a text via different output methods. self.say('tell me more.') :param req: text to output. :return: """ if screen: print(req) if say: # make request to google to get synthesis tts = gtts.gTTS(req, lang=self.LANG) # save the audio file tts.save("say.mp3") # play the audio file playsound("say.mp3") # proc = Popen(['spd-say', req]) # os.system(f'spd-say "{req}"') def listen(self): success = False recog_text_original = "" try: with sr.Microphone(device_index=self.microphone_index) as source: print("You may say Something") # self.play_sound("start_speech") # self.play_message("Řekněte, jakou možnost si přejete ..") # listens for the user's input audio = self.recognizer.listen( source, timeout=self.LISTENING_START_TIMEOUT, phrase_time_limit=self.PHRASE_TIMEOUT) # self.play_sound("end_speech") print("speech heard, processing...") # for testing purposes, we're just using the default API key # to use another API key, use `self.recognizer.recognize_google(audio, key="GOOGLE_SPEECH_RECOGNITION_API_KEY")` # speech recognition recog_text_original = self.recognizer.recognize_google( audio, language=self.LANG) print(recog_text_original) success = True except sr.UnknownValueError as e: # self.print_message(self.create_response("did_not_understand", locals()), block=True) print(f"Did not understand: {e}") except sr.WaitTimeoutError: # self.print_message(self.create_response("no_speech", locals())) print(f"No speech") else: # self.print_message(self.create_response("speech_recognition_result", locals())) print(f"I recognized text: {recog_text_original}") if success: return recog_text_original else: return "" def text_preprocessing(self, text_raw): # if success or (self.ALLOW_INTERRUPTIONS and recog_text_original): # processing recognized text recog_text = self.recText.replace_synonyms(text_raw) # self.sentences.append(recog_text) # rospy.sleep(1.0) msg = SentenceProgram() # msg.header.stamp = rospy.Time.now() msg.data.append(recog_text) # print("Publishing: {}".format(msg.data)) # self.pub.publish(msg) # self.whole = False # self.sentences = [] # print("Recognized text after synonyms substitution: ", recog_text) # load possible variants for next states # process recorded text using parser, tagging #tagged_text = self.get_tagged_text(recog_text) # TODO: more intelligent way of catching this return msg def add_object(self, type, x, y, z, id=None, color=None): req = AddObjectRequest() req.obj_class = type req.x = x req.y = y req.z = z if id: req.id = id if color: req.color = color req.action.action = DBOperation.ADD res = self._addsrv.call(req) assert isinstance(res, AddObjectResponse) return def get_database(self, write): req = GetDatabaseRequest() req.write = write res = self._getdatsrv.call(req) self.ontoC = self.db.change_onto(res.path) obj = self.ontoC.search(type=self.ontoC.Cube) if write: self.ontoC = self.db.onto.__enter__() return def send_database(self): req = SendDatabaseRequest() self.ontoC.__exit__() path = os.path.dirname( os.path.abspath(__file__)) + '/saved_updated_onto.owl' self.ontoC.save(path) # rospy.sleep(1.0) req.path = path res = self._senddatsrv.call(req) print(res.received.msg) print(res.received.success) # self.db.onto.__exit__() return def process_sentences(self, input_sentences): # populate the workspace #TODO should be replaced by input from a realsense if self.ADD_OBJECTS == 1: self.add_object('Glue', x=0.2, y=0.4, z=0, color='black') self.add_object('Panel', x=0.2, y=0.1, z=0, color='red') self.add_object('Panel', x=0.2, y=-0.2, z=0, color='blue') self.add_object('Cube', x=0.1, y=0.3, z=0, id='0', color='red') self.add_object('Cube', x=0.1, y=0.2, z=0, id='1', color='red') self.add_object('Cube', x=0.2, y=0.2, z=0, id='2', color='green') self.ADD_OBJECTS = 0 # self.add_object(onto.Screwdriver, x=0.1, y=0.3, z=0, id='0', color='red') # self.add_object('Screwdriver', x=0.1, y=0.2, z=0, id='1', color='red') # self.add_object('Screwdriver', x=0.2, y=0.2, z=0, id='2', color='green') # get current database after adding all objects self.get_database(write=False) # just to see if objects were added to the database obj = self.db.onto.search(type=db.onto.Cube) print(obj) obj = self.db.onto.search(type=db.onto.Glue) print(obj) obj = self.db.onto.search(type=db.onto.Panel) print(obj) for input_sentence in input_sentences: self.get_database(write=True) #self.ontoC = self.db.onto.__enter__() nl_processor = NLProcessor(language=self.LANG) program_template = nl_processor.process_text(input_sentence) # get current database state for writing an ungrounded and currently grounded program to be executed print() print("Program Template") print("--------") print(program_template) if self.db_api.get_state() == State.DEFAULT: # self.save_unground_program(program_template) self.send_database() self.get_database(write=True) # self.ontoC = self.db.onto.__enter__() robot_program = self.run_program(program_template) if self.db_api.get_state() == State.DEFAULT: self.save_grounded_program(robot_program) self.send_database() elif self.db_api.get_state() != State.LEARN_FROM_INSTRUCTIONS: self.db_api.set_state(State.DEFAULT) self.send_database() elif self.db_api.get_state() == State.LEARN_FROM_INSTRUCTIONS: self.save_new_template(program_template) self.send_database() # print list of programs self.get_database(write=False) all_custom_templates = self.db_api.get_custom_templates() for custom_template in all_custom_templates: print(custom_template.name[1:]) all_programs = self.db.onto.search(type=self.db.onto.RobotProgram) path = os.path.dirname( os.path.abspath(__file__)) + '/saved_updated_onto.owl' for program in all_programs: print(program.name) return def save_grounded_program(self, ground_program): # save to database and when database with the program sent, # we sent a message that the ground program was written to # database and new database sent # TODO search ontology for last program id # TODO link the corresponding ungrounded program in grounded one or vice versa # TODO add parameter time to the added program name = 'grounded_' + str(self.GroundedID) self.GroundedID = self.GroundedID + 1 self.db_api.save_program(ground_program, name) return def save_new_template(self, program_template): self.db_api.add_custom_template(program_template) self.db_api.set_state(State.DEFAULT) return def run_program(self, program_template): program_runner = ProgramRunner(language=self.LANG) robot_program = program_runner.evaluate(program_template) print() print("Grounded Program") print("--------") print(robot_program) return robot_program def callback(self, data): input_sentences = data.data self.process_sentences(input_sentences) return def run(self): """ The main function of this function is to hold the code execution. """ # embed() # this just pauses the code be entering IPython console, type "quit()" to quit or press ctrl+D # say "Hello World" # self.play_message("Slyšela jsem, že chceš postavit koně, teď se bude stavět.") while True: # key = self._readInput() # print(self._decode_states(self.next_state_possibilities.get_value())) # print(self.actual_state_number.get_value()) # print(self.mic_active.get_value()) # if key == "q": if 'q' in self.keys_pressed: self.print_message("User requested termination.") break elif 's' in self.keys_pressed: ui = UserInputManager(language=self.LANG) ui.query_state("shit on the string") print('detecting robot programs using ontology.') if not self.DEBUG_mode: self.say(self.guidance_file[self.LANG]["start_template"]) self.say(self.guidance_file[self.LANG]["start_specify"]) text = self.listen() if self.DEBUG_mode: if self.LANG == 'cs': # sentence_program.data = ["Polož kostka na pozici 3 3"] text = "Nalep bod sem a polož kostku sem." #text = "Ukliď červenou kostku." if self.LANG == 'en': text = "Glue a point here and Put cube to position 0 0" text = "Tidy up red cube." self.say(self.guidance_file[self.LANG]["debug_text"] + text) if text == "": self.play_message(self.create_response( "did_not_understand", locals()), block=True) self.print_message( self.create_response("no_speech", locals())) continue #raise NotImplementedError('What should happen when no text was recognized??') else: sentence_program = self.text_preprocessing(text) print(sentence_program) self.process_sentences(sentence_program.data) self.keys_pressed = set() print('processing complete') elif self.shouldProcessInput: # set mic_active back to False -> we started processing the input self.mic_active.set_value(False) # retrieve current and possible states # list of next state possibilities possible_states = self._decode_states( self.next_state_possibilities.get_value()) current_state = self.actual_state_number.get_value() # print(f"We are currently in state {current_state_text}.") #" and the possible choices are {text_possible_states}.") # self.play_message(f"Nyní jsme ve stavu {current_state_text}.") # možnosti k výběru jsou {text_possible_states}.") next_state = self.processInput( current_state, possible_states) # the chosen next state if next_state is not None and next_state > 0: try: # send state change request self._send_state(next_state) except Exception: print( "There was an error while sending the chosen next state. The error message was:" ) trace_exception() else: self.shouldProcessInput = False self.lastRecognitionFailed = False else: self.lastRecognitionFailed = True # recognition failed, attempt again on the next cycle continue def processInput(self, current_state, possible_states): """ This function should listen for audio and process the speech. It will be called only if user might enter an input. """ next_state_ID = -1 recog_text_original = "" [variants, possible_states, current_state_text] = self.get_variants(current_state, possible_states) if current_state_text == "": # no state hint, assuming passthrough state print("no state hint. Going to next state.") return possible_states[0] current_state_text, hint_directive = self.__extract_directive( current_state_text) if hint_directive == "-": # a passthrough state self.play_message(self.create_response("current_state", locals()), display=True) # automatically advance to the next state return possible_states[0] if not self.lastRecognitionFailed or self.repeatChoices: start_message = self.create_response("current_state", locals()) if not self.lastRecognitionFailed: if len(possible_states) > 1: start_message += self.create_response( "which_choice", locals()) elif hint_directive == "b": start_message += self.create_response( "yes_or_no", locals()) if self.ALLOW_INTERRUPTIONS: interrupted_text = self.play_message_and_listen(start_message, display=True) recog_text_original = interrupted_text + " " else: self.play_message(start_message, display=True) self.repeatChoices = False # breaking when above zero # if not self.block_until_sound(): # return -1 # speech to text from microphone success = False try: with sr.Microphone(device_index=self.microphone_index) as source: print("You may say Something") self.play_sound("start_speech") # self.play_message("Řekněte, jakou možnost si přejete ..") # listens for the user's input audio = self.recognizer.listen( source, timeout=self.LISTENING_START_TIMEOUT, phrase_time_limit=self.PHRASE_TIMEOUT) self.play_sound("end_speech") print("speech heard, processing...") # for testing purposes, we're just using the default API key # to use another API key, use `self.recognizer.recognize_google(audio, key="GOOGLE_SPEECH_RECOGNITION_API_KEY")` # speech recognition recog_text_original += self.recognizer.recognize_google( audio, language=self.LANG) # recog_text_original = 'velky velikost' except sr.UnknownValueError: self.play_message(self.create_response("did_not_understand", locals()), block=True) except sr.WaitTimeoutError: self.print_message(self.create_response("no_speech", locals())) else: self.print_message( self.create_response("speech_recognition_result", locals())) success = True if success or (self.ALLOW_INTERRUPTIONS and recog_text_original): # processing recognized text recog_text = self.recText.replace_synonyms(recog_text_original) # print("Recognized text after synonyms substitution: ", recog_text) # load possible variants for next states # process recorded text using parser, tagging tagged_text = self.get_tagged_text(recog_text) # TODO: more intelligent way of catching this #if ["moznosti", "možnost", "volby", "volba", "výběr"] in tagged_text: if "CHOICE_option" in tagged_text: self.repeatChoices = True return if len(variants) == 0: # TODO if nothing matching... print("No choices for this state. Is this an error?") return # select variant for the given actual state from possible next states if hint_directive == "b": #yes, no = [choice in tagged_text for choice in [ # ["ano", "muze", "jo", "preji", "chci","jasně","jasne","podej","dej","můžeš", "chtěla","můžete","muzete","muzes","podejte"], # ["ne", "nemuze","nechci","nedavej"] #]] #TODO yes is checked before no. What to do if both words? yes = "YES_option" in tagged_text no = "NO_option" in tagged_text # yes, no = [choice in tagged_text for choice in [ # self.synonyms_file[self.LANG]["YES_option"], # self.synonyms_file[self.LANG]["NO_option"] # ]] if yes and no: self.play_message( self.create_response("makes_no_sense", locals())) return elif yes: self.print_message("") return possible_states[0] elif no: self.play_message( self.create_response("no_cannot_proceed", locals())) return variantID, next_state = self.select_variant(tagged_text, variants) if variantID > -1: selected_variant = variants[variantID] next_state_ID = possible_states[variantID] self.print_message( self.create_response("selected_state_full", locals())) self.play_message( self.create_response("selected_state", locals())) else: print(f"Tagged text: {tagged_text}") print(f"Variants: {variants}") self.play_message( self.create_response("unknown_choice", locals())) return next_state_ID def create_response(self, response_id, local_variables={}, language=None): if "self" not in local_variables: local_variables["self"] = self response = "" try: response = self.responses[self.LANG if language is None else language][response_id].format( **local_variables) except KeyError as e: print(f"Variable missing from locals: {e}") print(f"Locals: {local_variables}") return response def print_message(self, message, raw=True): print(message) if self.SEND_ERRORS: self.response_string.set_value(message) def play_message(self, text, display=False, block=True): """Plays a message. The message is transformed into speech using TTS. Optionally, the message can be output to the console. Parameters ---------- text : str The text to be played as speech. display : bool, optional If true, the text is also displayed in the console (to remove the need for extra function calls). The *print_message* function is used for that. By default False """ if display: self.print_message(text) self.current_speech = SpeechThread(text, self.LANG, self.sox_effects) self.current_speech.start() if block: # self.block_until_sound(threshold=1, timeout=20) # if self.ALLOW_INTERRUPTIONS: # self.current_speech.terminate() # else: # self.current_speech.join() self.current_speech.join() def play_message_and_listen(self, text, display=False): self.play_message(text, display=display, block=False) while not self.current_speech.terminated: recog_text = "" try: with sr.Microphone( device_index=self.microphone_index) as source: audio = self.fast_recognizer.listen(source, phrase_time_limit=1) recog_text = self.fast_recognizer.recognize_google( audio, language=self.LANG) except Exception: continue else: if recog_text: return recog_text return "" def play_sound(self, sound_name): #cmd = ["sox", "-q", "-t", "wav", resource_filename("speech_vr", f"sounds/{sound_name}.wav")] playsound(resource_filename("speech_vr", f"sounds/{sound_name}.wav")) # cmd = ["mplayer", resource_filename("speech_vr", f"sounds/{sound_name}.wav")] # if sys.platform.startswith("win32"): # cmd.extend(("-t", "waveaudio")) # #cmd.extend(("gain", "-n", "vol", "1")) # subprocess.Popen(cmd) def block_until_sound(self, threshold=0.1, timeout=MICROPHONE_WAIT_TIMEOUT): # waiting for stream of data from microphone self._notbreak = True start_time = time() with sd.Stream(callback=lambda indata, outdata, frames, time, status, threshold=threshold: self.__print_sound( indata, outdata, frames, time, status, threshold), device=self.stream_device): while self._notbreak: # sd.sleep(0.1) if time() - start_time > timeout: self.print_message( self.create_response("no_audio", locals())) return False return True def __print_sound(self, indata, outdata, frames, time, status, threshold): volume_norm = np.linalg.norm(indata) * 10 # print(int(volume_norm)) if volume_norm > threshold: self._notbreak = False def get_variants(self, current_state, possible_states=[]): if self.USE_ACTIVE_STATE: # only based on the active_state print("Variant list: ", self.next_states_list) print(f"Current state ID: {current_state}") possible_states = self.next_states_list[self.LANG][str( current_state)] print('Loaded possible next states:', possible_states) # or based on the next_state_possibilities variants = [ self.variants_list[self.LANG][str(option)] for option in possible_states ] # text description of the current state current_state_text = self.state_hints_list[self.LANG][str( current_state)] if str(current_state) in self.state_hints_list[ self.LANG] else "" return variants, possible_states, current_state_text def load_files(self): # root_dir = os.path.join(os.path.dirname(os.path.abspath(__file__))) # variants_file = os.path.join(root_dir, "utils", "state_description_next.json") variants_file = resource_filename( "speech_vr", os.path.join("utils", "state_description.json")) with open(variants_file, "r", encoding="utf-8") as f: self.variants_list = json.load(f) if self.USE_ACTIVE_STATE: next_states_file = resource_filename( "speech_vr", os.path.join("utils", "next_states.json")) with open(next_states_file, "r", encoding="utf-8") as f: self.next_states_list = json.load(f) state_hints_file = resource_filename( "speech_vr", os.path.join("utils", "state_hints.json")) with open(state_hints_file, "r", encoding="utf-8") as f: self.state_hints_list = json.load(f) state_hints_file = resource_filename( "speech_vr", os.path.join("utils", "responses.json")) with open(state_hints_file, "r", encoding="utf-8") as f: self.responses = json.load(f) synonyms_file = resource_filename( "speech_vr", os.path.join("utils", "synonyms.json")) with open(synonyms_file, "r", encoding="utf-8") as f: self.synonyms_file = json.load(f) guidance_file = resource_filename( "speech_vr", os.path.join("utils", "guidance_dialogue.json")) with open(guidance_file, "r", encoding="utf-8") as f: self.guidance_file = json.load(f) def select_variant(self, tagged_text, variants): sel_variant_ID = -1 selected_variant = None # if len(variants) == 1: # sel_variant_ID = 0 # selected_variant = variants[0] # print('only one next state') # else: if self.STRIP_ACCENTS: variants = [unidecode(var) for var in variants] for variant_ID in range(len(variants)): # TODO handle multiple variants detection # TODO exchange for variants[variant_ID] in tagged_text - string works same, if list, if at least one in the list, gives back true if tagged_text.contains_pos_token(variants[variant_ID], '*'): print('Detected variant:', variants[variant_ID]) selected_variant = variants[variant_ID] sel_variant_ID = variant_ID return sel_variant_ID, selected_variant def get_tagged_text(self, recog_text): tagged_text = TaggedText() tagged_text.tokens = [] tagged_text.tags = [] tokens = nltk.word_tokenize(recog_text) # print(tokens) for pair in nltk.pos_tag(tokens): tag = Tag() tag.pos = POS(value=pair[1]) tagged_text.tokens.append( unidecode(pair[0]) if self.STRIP_ACCENTS else pair[0]) tagged_text.tags.append(tag) return tagged_text def datachange_notification(self, node, val, data): """ Subscription handler for "mic_active" """ if not val: return # the "mic_active" is False -> do nothing print("The mic is active!") # self.client.connect() # <- wtf point self.shouldProcessInput = True def event_notification(self, event): print("New event received: ", event) def _decode_states(self, next_state_values): if type(next_state_values[0]) is int: return next_state_values else: return [ v for v in [ int.from_bytes(b[:2], byteorder="little") for b in [ns.Body for ns in next_state_values] ] if v > 0 ] def _readInput(self, timeout=0.1): # start_time = time() # sys.stdout.write('%s(%s):' % (caption, default)) inp = '' if self.last_key == '': return '' if not isinstance(self.last_key, KeyCode): return '' # while True: # if msvcrt.kbhit(): # if kbhit(): # if self.last_key != '': # char = msvcrt.getche() if self.last_key.char == 'q': inp = self.last_key.char if self.last_key.char == 's': inp = self.last_key.char # char = getch.getche() # if ord(char) == 13: # enter_key # break # elif ord(char) >= 32: # space_char # inp = chr(ord(char)) # break # if len(inp) == 0 and (time() - start_time) > timeout: # break # # print('') # needed to move to next line self.last_key = '' if len(inp) > 0: return inp else: return "" def disconnect(self): try: self.sub.unsubscribe(self.handle) # cancel subscription self.sub.delete() except AttributeError: pass # not subscribed, yet except Exception: print("Error trying to unsubscribe from the mic_active variable!") self.client.disconnect()
""" import json import os from typing import List from pkg_resources import resource_filename from nlp_crow.database.DatabaseAPI import DatabaseAPI from nlp_crow.modules.CrowModule import CrowModule from nlp_crow.structures.tagging.MorphCategory import POS from nlp_crow.structures.tagging.ParsedText import TaggedText from nlp_crow.structures.tagging.Tag import Tag from nlp_crow.templates.TemplateFactory import TemplateType as tt from nlp_crow.modules.UserInputManager import UserInputManager db_api = DatabaseAPI() db = db_api.get_db() # onto = db_api.get_onto() # with onto: class TemplateDetector(CrowModule): """ First part of the NL pipeline. Preliminary template detection in the text. """ namespace = db.onto def __init__(self, language='en'): self.lang = language self.ui = UserInputManager(language=language)
class LearnTowerFromDemonstrationAction(Template): def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) self.logger = logging.getLogger(__name__) self.register_parameter(name="task_name", value=str) def match(self, tagged_text: TaggedText) -> None: match = tagged_text.match_regex(r"called (.*)") if match: self.task_name = match.group(1) else: uim = UserInputManager() self.task_name = uim.ask_for_name("tower") def evaluate(self) -> None: self.db_api = DatabaseAPI() self.db_api.set_param("current_task_name", self.task_name) dl = DemonstrationLearner() program = dl.create_new_program() last_cube_positions = {} for cube_positions in dl.get_cube_positions(): for key, val in cube_positions.items(): val self.add_object('Cube', x=0.1, y=0.3, z=0, id=key, color='red') logging.debug(f"Position of cubes: {cube_positions}") overlapping = self.detect_overlapping(last_cube_positions, cube_positions) if overlapping: bottom_cube_id = overlapping[0] top_cube_id = overlapping[1] logging.debug( f"Overlapping cubes detected: cube id {top_cube_id} is on top of cube id {bottom_cube_id}" ) if not program.root.children: # beginning of the demonstration, place the first cube self.add_pick_task(program, bottom_cube_id) self.add_put_first_cube_task(program, bottom_cube_id) self.add_pick_task(program, top_cube_id) self.add_put_task(program, top_cube_id, bottom_cube_id) last_cube_positions = cube_positions # time.sleep(1) logging.debug("Demonstration finished, adding program to database") logging.debug("\n" + str(program)) self.db_api.add_custom_template(program) self.db_api.set_state(State.LEARN_FROM_DEMONSTRATION) def detect_overlapping(self, last_cube_positions: Dict, cube_positions: Dict): cubes_disappeared = list( set(last_cube_positions.keys()).difference( set(cube_positions.keys()))) if cubes_disappeared: cube_disappeared_id = cubes_disappeared[ 0] # caring only about the first cube now cube_disappeared_pos = last_cube_positions[cube_disappeared_id] for cube_id, cube_pos in cube_positions.items(): if cube_pos.x == cube_disappeared_pos.x and cube_pos.y == cube_disappeared_pos.y: return [cube_disappeared_id, cube_id] return [] def add_pick_task(self, program, top_cube_id): pick_task = PickTask() top_cube = db.onto.ObjectPlaceholder() top_cube.is_a.append(db.onto.Cube) top_cube.id = top_cube_id pick_task.object_to_pick = top_cube self.add_task(program, pick_task) def add_put_task(self, program, top_cube_id, bottom_cube_id): put_task = PutTask() top_cube = db.onto.ObjectPlaceholder() top_cube.is_a.append(db.onto.Cube) top_cube.id = top_cube_id put_task.object_to_put = top_cube bottom_cube = db.onto.ObjectPlaceholder() bottom_cube.is_a.append(db.onto.Cube) bottom_cube.id = bottom_cube_id put_task.location = RelativeLocation() put_task.location.loc_type = "top" put_task.location.relative_to = bottom_cube self.add_task(program, put_task) def add_put_first_cube_task(self, program, cube_id): put_task = PutTask() top_cube = db.onto.ObjectPlaceholder() top_cube.is_a.append(db.onto.Cube) top_cube.id = cube_id put_task.object_to_put = top_cube db_api = DatabaseAPI() default_area = db_api.get_default_area() put_task.location = RelativeLocation() put_task.location.loc_type = "center" put_task.location.relative_to = default_area self.add_task(program, put_task) def add_task(self, program, template): node = RobotProgramOperand() node.template = template program.root.add_child(node)
def __init__(self, serverAdress, gain: int = 1.0): # instantiate client object; make sure address and port are correct self.DEBUG_mode = False self.client = Client(serverAdress) self.shouldProcessInput = True self.LANG = 'en' self.recognizer = sr.Recognizer() self.fast_recognizer = sr.Recognizer() self.fast_recognizer.non_speaking_duration = 0.05 self.fast_recognizer.pause_threshold = 0.15 self.recText = RawNLParser(language=self.LANG) self.lastRecognitionFailed = False self.repeatChoices = False self.last_key = '' self.keys_pressed = set() if self.DEBUG_mode: self.keys_pressed = 's' self.ADD_OBJECTS = 1 self.kb_listener = Listener(on_press=self.on_press) self.kb_listener.start() self.play_sound("start_speech") """ SoX Effects http://sox.sourceforge.net/sox.html#EFFECTS gain -n normalize the audio to 0dB -b balance the audio, try to prevent clipping dither adds noise to mask low sampling rate vol changes volume: above 1 -> increase volume below 1 -> decrease volume pad adds silence to the begining (first argument) and the end (second argument) of the audio - attempt to mask sound cut-off but extends the audio duration """ self.sox_effects = ("gain", "-n", "-b", "vol", str(gain), "pad", "0", "0.5", "dither", "-a") SpeechCustom.MAX_SEGMENT_SIZE = 300 Speech.MAX_SEGMENT_SIZE = 300 self.hint_directive_re = re.compile(r"<([b-])>\s*$") rospy.init_node('process_sentence_node', anonymous=False) logging.config.fileConfig(get_full_path('config', 'logging.ini')) self._getdatsrv = rospy.ServiceProxy('/db_interface_node/getDatabase', GetDatabase) req = GetDatabaseRequest() req.write = False res = self._getdatsrv.call(req) self.sub = rospy.Subscriber('/nl_input', SentenceProgram, queue_size=10, callback=self.callback) self.pub = rospy.Publisher('/sentence_output', SentenceProgram, queue_size=10) self._addsrv = rospy.ServiceProxy('/db_interface_node/addObject', AddObject) self._senddatsrv = rospy.ServiceProxy( '/db_interface_node/sendDatabase', SendDatabase) self.db_api = DatabaseAPI() self.db = self.db_api.get_db() self.ontoC = self.db.change_onto(res.path) # obj = self.db.onto.search(type = db.onto.Cube) # obj2 = self.ontoC.search(type = db.onto.Cube) # obj3 = self.db.onto.search(type = db.onto.Cube) # obj4 = self.ontoC.search(type = db.onto.Cube) # path = '/home/algernon/ros_melodic_ws/base_ws/src/crow_nlp/scripts/saved_updated_onto.owl' self.UngroundedID = 1 self.GroundedID = 1
def __init__(self): self.db_api = DatabaseAPI() self.ar = UserInputManager() self.og = ObjectGrounder() self.logger = logging.getLogger(__name__)
class execute_program_node(): def __init__(self): rospy.init_node('execute_program_node', anonymous=False) logging.config.fileConfig(get_full_path('config', 'logging.ini')) self._getdatsrv = rospy.ServiceProxy('/db_interface_node/getDatabase', GetDatabase) req = GetDatabaseRequest() req.write = False res = self._getdatsrv.call(req) # self.db = Database(res.path) self._addsrv = rospy.ServiceProxy('/db_interface_node/addObject', AddObject) self._senddatsrv = rospy.ServiceProxy('/db_interface_node/sendDatabase', SendDatabase) self.db_api = DatabaseAPI() # self.onto = self.db_api.get_onto() # path = '/home/algernon/ros_melodic_ws/base_ws/src/crow/database_com/nodes/saved_onto.owl' self.db = self.db_api.get_db() self.db.change_onto(res.path) obj = self.db.onto.search(type=self.db.onto.Cube) # exit_res = self.db.onto.__exit__() print('press a key to execute an action') self.robotProgram = 'grounded_1' self.listener = keyboard.Listener(on_press=self.on_press) self.listener.start() # rospy.on_shutdown(self.shutdown_hook) def on_press(self,key): """ Executes the robotic program """ print('hi') self.get_database(write=False) all_programs = self.db.onto.search(type=self.db.onto.RobotProgram) program_grounded_all = [x for x in all_programs if x.name.startswith("grounded")] for program_grounded in program_grounded_all: print(program_grounded.name) print(program_grounded) program_executor = ProgramExecutor() program_executor.evaluate(program_grounded) return def get_database(self, write): req = GetDatabaseRequest() # self.ontoC = self.db.onto.__exit__() req.write = write res = self._getdatsrv.call(req) # self.db_api = DatabaseAPI() # self.db = self.db_api.get_db() self.db.change_onto(res.path) #path = '/home/algernon/ros_melodic_ws/base_ws/src/crow_nlp/scripts/saved_updated_onto.owl' #self.ontoC = self.db.change_onto(path) self.db.onto.__enter__() obj = self.db.onto.search(type=self.db.onto.Cube) self.db.onto.__exit__() # self.ontoC = self.db.onto.__enter__() print('hi') return
from nlp_crow.database.DatabaseAPI import DatabaseAPI from nlp_crow.database.Ontology import Template from nlp_crow.modules.LocationDetector import LocationDetector import owlready2 as ow from nlp_crow.modules.LocationGrounder import LocationGrounder from nlp_crow.modules.ObjectDetector import ObjectDetector from nlp_crow.modules.ObjectGrounder import ObjectGrounder from nlp_crow.structures.tagging.TaggedText import TaggedText from nlp_crow.modules.UserInputManager import UserInputManager import logging db = Database() db_api = DatabaseAPI() # with db.onto as onto: class PutTask(Template): namespace = db.onto def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) self.register_parameter(name="object_to_put", value=db.onto.Object) self.register_parameter(name="location", value=db.onto.Location) self.logger = logging.getLogger(__name__) self.ui = UserInputManager() self.templ_det = self.ui.load_file('templates_detection.json')
class process_sentence_node(): # listens to /averaged_markers from object_detection package and in parallel to language input # (from microphone or from keyboard). For each received sentence, a program template and grounded program is created # via communication with database def __init__(self): rospy.init_node('process_sentence_node', anonymous=False) logging.config.fileConfig(get_full_path('config', 'logging.ini')) self._getdatsrv = rospy.ServiceProxy('/db_interface_node/getDatabase', GetDatabase) req = GetDatabaseRequest() req.write = False res = self._getdatsrv.call(req) self.sub = rospy.Subscriber('/nl_input', SentenceProgram, queue_size=10, callback=self.callback) self.pub = rospy.Publisher('/sentence_output', SentenceProgram, queue_size=10) self._addsrv = rospy.ServiceProxy('/db_interface_node/addObject', AddObject) self._senddatsrv = rospy.ServiceProxy( '/db_interface_node/sendDatabase', SendDatabase) self.db_api = DatabaseAPI() self.db = self.db_api.get_db() self.ontoC = self.db.change_onto(res.path) #obj = self.db.onto.search(type = db.onto.Cube) #obj2 = self.ontoC.search(type = db.onto.Cube) #obj3 = self.db.onto.search(type = db.onto.Cube) #obj4 = self.ontoC.search(type = db.onto.Cube) #path = '/home/algernon/ros_melodic_ws/base_ws/src/crow_nlp/scripts/saved_updated_onto.owl' self.UngroundedID = 1 self.GroundedID = 1 def callback(self, data): input_sentences = data.data self.process_sentences(input_sentences) return def process_sentences(self, input_sentences): # populate the workspace #TODO should be replaced by input from a realsense self.add_object('Glue', x=0.2, y=0.4, z=0, color='black') self.add_object('Panel', x=0.2, y=0.1, z=0, color='red') self.add_object('Panel', x=0.2, y=-0.2, z=0, color='blue') self.add_object('Cube', x=0.1, y=0.3, z=0, id='0', color='red') self.add_object('Cube', x=0.1, y=0.2, z=0, id='1', color='red') self.add_object('Cube', x=0.2, y=0.2, z=0, id='2', color='green') # get current database after adding all objects self.get_database(write=False) # just to see if objects were added to the database obj = self.db.onto.search(type=db.onto.Cube) print(obj) obj = self.db.onto.search(type=db.onto.Glue) print(obj) obj = self.db.onto.search(type=db.onto.Panel) print(obj) for input_sentence in input_sentences: self.get_database(write=True) #self.ontoC = self.db.onto.__enter__() nl_processor = NLProcessor() program_template = nl_processor.process_text(input_sentence) # get current database state for writing an ungrounded and currently grounded program to be executed print() print("Program Template") print("--------") print(program_template) if self.db_api.get_state() == State.DEFAULT: # self.save_unground_program(program_template) self.send_database() self.get_database(write=True) # self.ontoC = self.db.onto.__enter__() robot_program = self.run_program(program_template) if self.db_api.get_state() == State.DEFAULT: self.save_grounded_program(robot_program) self.send_database() elif self.db_api.get_state() != State.LEARN_FROM_INSTRUCTIONS: self.db_api.set_state(State.DEFAULT) self.send_database() elif self.db_api.get_state() == State.LEARN_FROM_INSTRUCTIONS: self.save_new_template(program_template) self.send_database() # print list of programs self.get_database(write=False) all_custom_templates = self.db_api.get_custom_templates() for custom_template in all_custom_templates: print(custom_template.name[1:]) all_programs = self.db.onto.search(type=self.db.onto.RobotProgram) path = os.path.dirname( os.path.abspath(__file__)) + '/saved_updated_onto.owl' for program in all_programs: print(program.name) return def act(self, program_template): state = self.db_api.get_state() if state == State.DEFAULT: self.run_program(program_template) elif state == State.LEARN_FROM_INSTRUCTIONS: self.db_api.add_custom_template(program_template) self.db_api.set_state(State.DEFAULT) def run_program(self, program_template): program_runner = ProgramRunner() robot_program = program_runner.evaluate(program_template) print() print("Grounded Program") print("--------") print(robot_program) return robot_program def save_new_template(self, program_template): self.db_api.add_custom_template(program_template) self.db_api.set_state(State.DEFAULT) return def save_unground_program(self, program_template): # save to database and when database with the program sent, # we sent a message that the ground program was written to # database and new database sent # TODO add parameter time to the added program # TODO add parameter to be done to the added program # TODO search ontology for last program id name = 'ungrounded_' + str(self.UngroundedID) self.UngroundedID = self.UngroundedID + 1 self.db_api.save_program(program_template, name) return def save_grounded_program(self, ground_program): # save to database and when database with the program sent, # we sent a message that the ground program was written to # database and new database sent # TODO search ontology for last program id # TODO link the corresponding ungrounded program in grounded one or vice versa # TODO add parameter time to the added program name = 'grounded_' + str(self.GroundedID) self.GroundedID = self.GroundedID + 1 self.db_api.save_program(ground_program, name) return def get_database(self, write): req = GetDatabaseRequest() req.write = write res = self._getdatsrv.call(req) self.ontoC = self.db.change_onto(res.path) obj = self.ontoC.search(type=self.ontoC.Cube) if write: self.ontoC = self.db.onto.__enter__() return def send_database(self): req = SendDatabaseRequest() self.ontoC.__exit__() path = os.path.dirname( os.path.abspath(__file__)) + '/saved_updated_onto.owl' self.ontoC.save(path) # rospy.sleep(1.0) req.path = path res = self._senddatsrv.call(req) print(res.received.msg) print(res.received.success) # self.db.onto.__exit__() return def add_object(self, type, x, y, z, id=None, color=None): req = AddObjectRequest() req.obj_class = type req.x = x req.y = y req.z = z if id: req.id = id if color: req.color = color req.action.action = DBOperation.ADD res = self._addsrv.call(req) assert isinstance(res, AddObjectResponse) return
class BasicTest(unittest.TestCase): def setUp(self): self.db = DatabaseAPI() self.onto = self.db.get_onto() def test_add_and_remove_tools(self): with self.onto as onto: # populate the workspace self.db.add_object(onto.Screwdriver, x=1, y=2, color="red") self.db.add_object(onto.Screwdriver, x=3, y=1, color="blue") self.db.add_object(onto.Hammer, x=4, y=4, color="blue") self.db.add_object(onto.Pliers, x=3, y=3, color="green") # try a few queries about the workspace print("All objects in the workspace:") pp(self.db.get_all_tools()) print() print("All screwdrivers in the workspace:") pp(self.db.get_class_objects(onto.Screwdriver)) print() print("All blue objects in the workspace:") pp(self.db.get_objects_by_color("blue")) print() # # update position of the pliers # pliers = self.db.get_class_objects(onto.Pliers)[0] # self.db.update_object_position(pliers, x=5, y=4) # print("The pliers after updating its position:") # print(pliers) # print() # delete the nail print("Deleting the pliers...") self.db.delete_object(pliers) print("List of pliers:") pp(self.db.get_class_objects(onto.Pliers)) print() # process a NL instruction nl_processor = NLProcessor() input_sentence = "Now take the screwdriver and put it in the right corner." robot_program = nl_processor.process_text(input_sentence) print() print("Program") print("--------") print(robot_program)
class update_object_position_node(): # listens to /averaged_markers from object_detection package and in parallel to button presses. When there is an approval # to collect cubes (key a -> y), robot picks up the currently detected cubes and places them to storage def __init__(self): rospy.init_node('db_interface_node', anonymous=False) self._dbapi = DatabaseAPI() self.db = self._dbapi.get_db() self.sub_cubes = rospy.Subscriber('/averaged_markers', MarkersAvgList, self.update_object_position) # self.listener = keyboard.Listener(on_press=self.on_press) # self.listener.start() self.pub = rospy.Publisher('/database_update', DatabaseUpdate, queue_size=10) self.srv = {} # self.srv['groundProgram'] = rospy.Service('~groundProgram', GroundProgram, self.handle_ground_program) self.srv['addObject'] = rospy.Service('~addObject', AddObject, self.handle_add_object) self.srv['getDatabase'] = rospy.Service( '~getDatabase', GetDatabase, self.handle_get_database_request) self.srv['sendDatabase'] = rospy.Service( '~sendDatabase', SendDatabase, self.handle_send_database_request) self.database_locked = False # rospy.on_shutdown(self.shutdown_hook) def handle_add_object(self, req): """ Updates the position of the existing object in database or adds the object if it does not exist yet. Service Parameters ---------- req.obj_class ontology class of the object to be added, string (e.g. 'Cube') req.id the identifier of the object, e.g. ArUco id, string req.x the position of the object: x coordinate, float64 req.y the position of the object: y coordinate, float64 req.color color of the object: string req.DBOperation action to be performed (e.g. 'ADD' adds an object to database) res.res.msg message, string res.res.success True if successful, Bool """ assert isinstance(req, AddObjectRequest) res = AddObjectResponse() obj = self.db.onto.search_one(id=req.id, _is_in_workspace=True) choices = { 'Cube': self.db.onto.Cube, 'Panel': self.db.onto.Panel, 'Glue': self.db.onto.Glue } typeO = choices.get(req.obj_class, 'Cube') self.db.onto.__enter__() if obj is None: self._dbapi.add_object(Class=typeO, x=req.x, y=req.y, z=req.z, id=req.id, color=req.color) res.res.msg = 'I added a {} {} with id {} at x = {}, y = {} and z = {}'.format( req.color, req.obj_class, req.id, req.x, req.y, req.z) res.res.success = True else: #TODO if change of position > threshold update position obj.location.x = req.x obj.location.y = req.y obj.location.z = req.z res.res.msg = 'I updated the position of a {} {} with id {} from x = {}, y = {}, z = {} to x = {}, y = {} and z = {}'.format( req.color, req.obj_class, req.id, obj.location.x, obj.location.y, req.x, req.y, req.z) res.res.success = True #TODO update other parameters if changed... # TODO test whether the object arrived in the database obj = self.db.onto.search(id=req.id, _is_in_workspace=True) print(obj) self.db.onto.__exit__() return res def handle_get_database_request(self, req): # type: (GetDatabaseRequest) -> GetDatabaseResponse """ Receives the request for writing/reading a current database saves current database, locks it and send a path to it Service Parameters ---------- req.write 1 if external node wants to write to the database (needs to be locked), 0 if only for reading (does not have to be locked)service request res.path path to the saved current database """ path = os.path.dirname(os.path.abspath(__file__)) + '/saved_onto.owl' self.db.onto.save(path) if req.write: if self.database_locked == False: #TODO lock database self.database_locked = True print('write') else: #TODO how to queue request? print('Cannot process request, database sent to other request') else: #do not lock and continue happily writing to it print('read') res = GetDatabaseResponse() res.path = path return res def handle_send_database_request(self, req): # type: (SendDatabaseRequest) -> SendDatabaseResponse """ Receives the path to database after being updated externaly And updates and unlocks the current database Service Parameters ---------- req.path service request contains path to the new database res.received.success True if loading new database was successfull res.received.msg message about process """ #TODO how to check that if database is sent it can be uploaded here - # that it was locked and we do not overwrite changes or other service # is not having it new_onto_path = req.path #TODO database_locked before res = SendDatabaseResponse() if self.database_locked: #receives path to the updated database and loads # it instead of the current one self.db.change_onto( new_onto_path ) #TODO if error in changing onto -> success false, msg that could not be loaded res.received.success = True res.received.msg = 'Database received and loaded successfully' obj = self.db.onto.search(type=self.db.onto.RobotProgram) print(obj) #TODO unlock database self.database_locked = False else: #cannot process request because current database # was not locked - changes might be made to it in between #TODO how to handle merge? res.received.success = False res.received.msg = 'Database was not locked, it could not be overwritten by the received one thanks to potential conflicts' return res def update_object_position(self, data): #def update_object_position(self, id: str, x: float, y: float): """ Updates the position of the existing object or adds the object if it does not exist yet. Parameters ---------- id the identifier of the object, e.g. ArUco id x the position of the object: x coordinate y the position of the object: y coordinate """ if len(data.markers) > 0: # if pub is None: # pub = rospy.Publisher('/database_update', DatabaseUpdate, queue_size=10) msg = DatabaseUpdate() msg.header = data.header # pose_transformed_obj.header msg.id = [] msg.update_id = [] for i in range(0, len(data.markers)): pose = data.markers[i].avg_pose id = data.markers[i].id #obj = onto.search(aruco_id=1,_is_in_workspace=True) obj = self.db.onto.search(type=self.db.onto.Cube) addObj = True for i in range(0, len(obj)): if (obj[i].aruco_id == id) and (obj[i]._is_in_workspace == True): addObj = False idx = i #obj = True changed_position = True if addObj: # creates a new cube # TODO make it work with any object self.add_object(self.db.onto.Cube, x=pose.position.x, y=pose.position.y, id=id) print( self.db.onto.search(type=self.db.onto.Cube, _is_in_workspace=True)) msg.id.append(id) msg.update_id.append(1) elif changed_position and not addObj: obj[idx].location.x = pose.position.x obj[idx].location.y = pose.position.y msg.id.append(id) msg.update_id.append( 2) # should be 2 only if pose actually updated self.pub.publish(msg) if len(data.markers) == 0: return def add_object(self, Class: ClassVar, x: float, y: float, id: str = None, color: str = None): """ Adds a new object in the ontology. Parameters ---------- Class the class of the ontology object to be added, sho Class: ClassVar, x : float, y : float, id : str = None, color : str = Noneuld be a subclass of onto.Object x the position of the object: x coordinate y the position of the object: y coordinate id the identifier of the object, e.g. ArUco id color the color of the object as string, e.g. "red" ------- """ obj = Class() if id: obj.aruco_id = id if color: color = self.db.onto.NamedColor(color) obj.color.append(color) point = self.db.onto.Point2D(x=x, y=y) obj.location = point # TODO without this, some objects may be involuntarily destroyed by a garbage collector # TODO this should be investigated further to avoid some magical errors self.db.objects.append(obj) # set the internal attribute which signifies this is a real object in the workspace obj._is_in_workspace = True
def setUp(self): self.db = DatabaseAPI() self.onto = self.db.get_onto()
def __init__(self): self.db_api = DatabaseAPI() self.cd = ColorDetector() self.ar = UserInputManager() self.logger = logging.getLogger(__name__)
def __init__(self): self.logger = logging.getLogger(__name__) self.db_api = DatabaseAPI()