diff --git a/route_guidance_ros/requirements.txt b/route_guidance_ros/requirements.txt index 5221619b49c012fa89be27dc4a132e10dd033404..5495fb3ee4409c30ef9d04b686c1757f8ddf1bfb 100644 --- a/route_guidance_ros/requirements.txt +++ b/route_guidance_ros/requirements.txt @@ -1,2 +1,4 @@ pyrois numpy +termcolor +transitions diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py new file mode 100644 index 0000000000000000000000000000000000000000..66923e746e07b324c31b992473d7281ae1678692 --- /dev/null +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py @@ -0,0 +1,180 @@ +# (Dummy) Speech_Recognition.py +# + +"""Speech_Recognition +""" + +import sys +import queue +import time +from datetime import datetime +import threading + +from pyrois import RoIS_Common, RoIS_HRI + +if sys.version_info.major == 2: + import SocketServer + import SimpleXMLRPCServer + + class ThreadingXMLRPCServer(SocketServer.ThreadingMixIn, SimpleXMLRPCServer.SimpleXMLRPCServer): + pass +else: + import socketserver + import xmlrpc.server + + class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer): + """ThreadingXMLRPCServer + """ + pass + + +class Command(RoIS_Common.Command): + """Command + """ + def __init__(self, c): + self._component = c + + def start(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def stop(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def suspend(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def resume(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def set_parameter(self, languages, grammer, rule): + self._component.Recognizable_Languages = recognizable_languages + self._component.Languages = languages + self._component.Grammer = grammer + self._component.Rule = rule + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, c): + self._component = c + + def component_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + c_status = RoIS_Common.Component_Status.UNINITIALIZED.value + return (status, c_status) + + def get_parameter(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return (status, self._component.Languages, self._component.Grammer, self._component.Rule) + + +class Event(RoIS_Common.Event): + """Event + """ + def __init__(self, c): + self._component = c + self.event_queue = queue.Queue() + + def poll_event(self): + """poll_event + """ + msg = self.event_queue.get() + return msg + + def speech_recognized(self, timestamp, recognized_text): + """speech_recognition + """ + msg = xmlrpc.client.dumps((timestamp, recognized_text), 'speech_recognized') + self.event_queue.put(msg) + + def speech_input_started(self, timestamp): + msg = xmlrpc.client.dumps((timestamp), 'speech_input_started') + self.event_queue.put(msg) + + def speech_input_finished(self, timestamp): + msg = xmlrpc.client.dumps((timestamp), 'speech_input_finished') + self.event_queue.put(msg) + +import random + +class Speech_Recognition(Event, Command, Query): + """Speech_Recognition + """ + def __init__(self, c): + super().__init__(c) + self._component = c + self._component.Recognizable_Languages = set("") + self._component.Languages = set("English") + self._component.Grammer = "" #??? + self._component.Rule = "" #??? + + def set_parameter(self, languages, grammer, rule): + print("Speech_Recognition::set_parameter called.") + th = threading.Thread( + target=self.wait_and_send_speech_recognized_event, + daemon=True) + th.start() + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def wait_and_send_speech_recognized_event(self): + detected_text = "point_a" + delay_duration = random.randint(3, 10) + time.sleep(delay_duration) + self.speech_recognized(datetime.now(), [detected_text]) + + def speech_recognized(self, timestamp, recognized_text): + """speech_recognition + """ + msg = (timestamp, recognized_text), 'speech_recognized' + msg = xmlrpc.client.dumps(*msg) + self.event_queue.put(msg) + + +class component: + """component + """ + def __init__(self): + self._state = False + + +def event_dispatch(sr): + """event_dispatch + """ + sr.speech_recognized(datetime.now().isoformat(), [""]) + time.sleep(0.5) + sr.speech_recognized(datetime.now().isoformat(), [""]) + + +def example_sr(port): + """example_sr + """ + c = component() + sr = Speech_Recognition(c) + + # start the timer to dispatch events + t = threading.Timer(3, event_dispatch, args=(sr,)) + t.start() + + # start the XML-RPC server + server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) + server.register_instance(sr) + server.register_introspection_functions() + server.register_multicall_functions() + server.serve_forever() + + +if __name__ == '__main__': + if len(sys.argv) < 2: + print("python3 ") + raise RuntimeError + + port = int(sys.argv[1]) + example_sr(port) diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py new file mode 100644 index 0000000000000000000000000000000000000000..d1280d8821dc876339ca549508308e93a6ea9422 --- /dev/null +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py @@ -0,0 +1,125 @@ +# (Dummy) Speech_Recognition_Client.py + +"""Speech_Recognition_Client +""" + +import threading +# import logging +import xmlrpc.client + +from pyrois import RoIS_Common, RoIS_HRI + + +class Command(RoIS_Common.Command): + """Command + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def start(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.start()) + return status + + def stop(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.stop()) + return status + + def suspend(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.suspend()) + return status + + def resume(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.resume()) + return status + + def set_parameter(self, languages, grammer, rule): + s = self._proxy.set_parameter(languages, grammer, rule) + status = RoIS_HRI.ReturnCode_t(s) + return status + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def component_status(self): + (status, c_status) = self._proxy.component_status() + return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status)) + + def get_parameter(self): + (status, recognizable_languages, languages, grammer, rule) = self._proxy.get_parameter() + return (RoIS_HRI.ReturnCode_t(status), recognizable_languages, languages, grammer, rule) + + +class Event(RoIS_Common.Event): + """Event + """ + def __init__(self, uri): + self._uri = uri + self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + self.events = [] + # if logger is not None: + # self.logger = logger + + def start_th(self): + self.th = threading.Thread(target=self.event_loop) + self.th.start() + + def event_loop(self): + """event_loop + """ + while True: + try: + self.poll_event() + except ConnectionRefusedError: + break + + def poll_event(self): + """poll_event + """ + msg = self._e_proxy.poll_event() + (params, methodname) = xmlrpc.client.loads(msg) + #self.logger.debug('poll_event: '+methodname) + print(params,methodname) + if methodname == 'speech_recognized': + self.speech_recognized(params[0], params[1]) + + def speech_recognized(self, timestamp, recognized_text): + self.events.append((timestamp,recognized_text)) + print("speech_recognized",timestamp, recognized_text) + # self.logger.debug('received completed event' + # + command_id + # + RoIS_Service.Completed_Status(status).name) + + def speech_input_started(self, timestamp): + self.events.append((timestamp)) + print("speech_input_started", timestamp) + + def speech_input_finished(self, timestamp): + self.events.append((timestamp)) + print("speech_input_started", timestamp) + + +class Speech_Recognition_Client(Command, Query, Event): + """Speech_Recognition_Client + """ + def __init__(self, uri, engine_event_queue): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + self.events = [] + self.engine_event_queue = engine_event_queue + self.start_th() + + def poll_event(self): + """poll_event + """ + msg = self._e_proxy.poll_event() + (params, methodname) = xmlrpc.client.loads(msg) + print("[Speech_Recognition_Client::poll_event]", params, methodname) + msg = xmlrpc.client.dumps((methodname, params), 'completed') + self.engine_event_queue.put(msg) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 3d7d651bc106fe712e708067e27520a5c7b59c4a..bf7a35a8288f459fce08c279e0a7376c9ba3b392 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +# -*- coding: utf-8 -*- # VirtualNavigation.py # @@ -86,9 +87,15 @@ class VirtualNavigation(Event, Command, Query): self._component.Target_Position = [""] self._component.Time_Limit = 10 self._component.Routing_Policy = "" + self.init_pos_table = { + 'robot1': [0.0, 0.0, 0.0], + 'robot2': [10.0, 0.0, 0.0], + 'robot3': [0.0, 10.0, 0.0] + } self.robot_goal_table = {} self.nav_state_table = \ - {robot_name : Component_Status.UNINITIALIZED for robot_name in robot_names} + {robot_name : Component_Status.READY for robot_name in robot_names} + # {robot_name : Component_Status.UNINITIALIZED for robot_name in robot_names} self.pointname_coord_table = yaml.load(open("field_points.yaml")) def position_to_coord(self, position): @@ -98,8 +105,12 @@ class VirtualNavigation(Event, Command, Query): return position def set_parameter(self, target_position, time_limit, routing_policy): + # ERRORなロボットは初期位置に戻す + self.back_stucked_robots_to_init_pos() + + # READYなロボットの中でゴールに最も近い位置にいるものにNavigationさせる target_coord = self.position_to_coord(target_position) - nearest_robot = self._component.get_nearest_robotname(target_coord) + nearest_robot = self._component.get_nearest_robotname(target_coord, self.nav_state_table) th = threading.Thread( target=self.send_goal_and_event, args=(target_position, nearest_robot)) @@ -110,10 +121,29 @@ class VirtualNavigation(Event, Command, Query): def send_goal_and_event(self, goal, robot_name): goal_coord = self.position_to_coord(goal) + self.nav_state_table[robot_name] = Component_Status.BUSY + print(self.nav_state_table) status = self._component.send_goal_to_robot_async(goal_coord, robot_name) - msg = xmlrpclib.dumps((goal, robot_name), 'completed') + self.nav_state_table[robot_name] = status + print(self.nav_state_table) + msg = xmlrpclib.dumps((goal, robot_name, status.value), 'navi_finished') self.event_queue.put(msg) + def back_stucked_robots_to_init_pos(self): + def reset_robot_pos_and_state(robot_name): + init_pos = self.init_pos_table[robot_name] + init_pos = self.position_to_coord(init_pos) + self._component.send_goal_to_robot_async(init_pos, robot_name) + self.nav_state_table[robot_name] = Component_Status.READY + + for robot_name, state in self.nav_state_table.items(): + if state == Component_Status.ERROR: + th = threading.Thread( + target=reset_robot_pos_and_state, + args=(robot_name, )) + th.daemon = True + th.start() + def event_dispatch(n): # n.person_detected(datetime.now().isoformat(), 1) @@ -142,19 +172,23 @@ class Component(object): pos = amcl_pose.pose.pose.position self.__robot_positions[robot_name] = np.array([pos.x, pos.y, pos.z]) - def send_goal_to_nearest_robot(self, dest): + def send_goal_to_nearest_robot(self, dest, state_table): dest = np.array(dest) - distance_table = \ - {name: np.linalg.norm(dest - pos) for name, pos in self.__robot_positions.items()} + distance_table = {} + for name, pos in self.__robot_positions.items(): + distance_table[name] = np.linalg.norm(dest - pos) \ + if state_table[name] == Component_Status.READY else float('inf') nearest_robot = min(distance_table, key=distance_table.get) print("nearest_robot is ", nearest_robot) self.__goal_senders[nearest_robot].send_goal(dest.tolist()) return nearest_robot - def get_nearest_robotname(self, dest): + def get_nearest_robotname(self, dest, state_table): dest = np.array(dest) - distance_table = \ - {name: np.linalg.norm(dest - pos) for name, pos in self.__robot_positions.items()} + distance_table = {} + for name, pos in self.__robot_positions.items(): + distance_table[name] = np.linalg.norm(dest - pos) \ + if state_table[name] == Component_Status.READY else float('inf') nearest_robot = min(distance_table, key=distance_table.get) return nearest_robot diff --git a/route_guidance_ros/scripts/goal_sender_ros.py b/route_guidance_ros/scripts/goal_sender_ros.py index 13bec919b398c04319d4ba86118f6f66dff21b04..df0592a34f247d55ab56e2a9ce5082faa1f75d29 100644 --- a/route_guidance_ros/scripts/goal_sender_ros.py +++ b/route_guidance_ros/scripts/goal_sender_ros.py @@ -50,11 +50,12 @@ class GoalSenderROS(object): def send_goal_and_wait(self, pose_xy): pose = [pose_xy, (0.0, 0.0, 0.0, 1.0)] goal = pose_to_goal(pose) - return self.action_client.send_goal_and_wait(goal) + state = self.action_client.send_goal_and_wait(goal) + return GoalSenderROS.GoalSenderState[state] def get_goalsender_state(self): - state = self.action_client.get_state() - return GoalSenderROS.GoalSenderState[state] + state = self.action_client.get_state() + return GoalSenderROS.GoalSenderState[state] # If you remove this comment out, it does not work. # rospy.init_node('goal_sender') diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 63fbcee9bf25f8db6d6cc2c5aa11d973d9934d61..eb2f3f9d55e765fa85a2899e4ee075f386808470 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -169,9 +169,12 @@ class EventIF(RoIS_HRI.EventIF): return (status, results) from VirtualNavigation_client import VirtualNavigation_Client as VNavClient +from Dummy_Speech_Recognition_client import Speech_Recognition_Client as SRecogClient from sub_engine_client import IF as EngineClient from pyrois.RoIS_Common import Component_Status +from pyrois.RoIS_Service import Completed_Status import time +from termcolor import colored class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): """IF @@ -182,23 +185,19 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.event_queue = queue.Queue() self.component_event_queue = queue.Queue() self.component_clients = { - 'Navigation': VNavClient('http://localhost:8041', self.component_event_queue) + 'Navigation': VNavClient('http://localhost:8041', self.component_event_queue), + 'Speech_Recognition': SRecogClient('http://localhost:8042', self.component_event_queue) } self.engine_clients = { 'robot1': EngineClient('http://localhost:8010'), 'robot2': EngineClient('http://localhost:8020'), 'robot3': EngineClient('http://localhost:8030') } - self.init_pos_table = { - 'robot1': (0.0, 0.0 ), - 'robot2': (10.0, 0.0 ), - 'robot3': (0.0, 10.0) - } self.goal_command_table = {} for engine_client in self.engine_clients.values(): engine_client.connect() self.command_id_result_table = {} - th = threading.Thread(target=self.analyze_vnav_status, daemon=True) + th = threading.Thread(target=self.analyze_c_status, daemon=True) th.start() def set_parameter(self, component_ref, *parameters): @@ -216,21 +215,33 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): status, results = engine_client.get_command_result(command_id, "") self.command_id_result_table[str(self.command_id)].append(results) status = RoIS_HRI.ReturnCode_t.OK.value + elif component_ref == 'Speech_Recognition': + pass + return (status, str(self.command_id)) def get_command_result(self, command_id, condition): status = RoIS_HRI.ReturnCode_t.OK.value return status, self.command_id_result_table[command_id] - def analyze_vnav_status(self): + def analyze_c_status(self): while True: msg = self.component_event_queue.get() - (target_pos, robot_name), methodname = xmlrpc.client.loads(msg) + (target_pos, robot_name, status), methodname = xmlrpc.client.loads(msg) command_id = self.goal_command_table[str(target_pos)] - print("Command (id: {}) was completed by {} which reached goal {}."\ - .format(command_id, robot_name, target_pos)) - self.completed(command_id, Component_Status.READY.value) - del self.goal_command_table[str(target_pos)] + status = Component_Status(status) + if status == Component_Status.READY: + print(colored("Command (id: {}) was succeeded by {} which reached goal {}."\ + .format(command_id, robot_name, target_pos), "green")) + self.completed(command_id, Completed_Status.OK.value) + del self.goal_command_table[str(target_pos)] + time.sleep(3) + elif status == Component_Status.ERROR: + print(colored("Command (id: {}) was aborted by {} which couldn't reach goal {}."\ + .format(command_id, robot_name, target_pos), "red")) + self.completed(command_id, Completed_Status.ABORT.value) + else: + raise RuntimeError("Unhandled kind of vnav-status.") time.sleep(3) diff --git a/route_guidance_ros/scripts/service_app_lv1.py b/route_guidance_ros/scripts/service_app_lv1.py index 5a220f816617ce71873608e6f9505e8d548de31f..56abfeff78d201943344d24a701e353539f8a68f 100644 --- a/route_guidance_ros/scripts/service_app_lv1.py +++ b/route_guidance_ros/scripts/service_app_lv1.py @@ -2,6 +2,7 @@ from pyrois.Service_Application_IF import Service_Application_IF from pyrois import RoIS_Service, RoIS_HRI from utilities import setup_multi_robot from pyrois.RoIS_Common import Component_Status +from pyrois.RoIS_Service import Completed_Status import logging import queue @@ -24,7 +25,7 @@ class TaskState(object): TaskKind.End: TaskKind.End } - def __init__(self, task_kind, status=Component_Status.BUSY): + def __init__(self, task_kind, status=None): self.task_kind = task_kind self.status = status @@ -44,7 +45,7 @@ class Service(Service_Application_IF): def completed(self, command_id, status): task = self.commandid_taskstate_table[command_id] - status = Component_Status(status) + status = Completed_Status(status) task.status = status next_task = task.next() if next_task == TaskKind.NaviToGuide: diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py new file mode 100644 index 0000000000000000000000000000000000000000..f5b54273711a088cddf7e37f0d967288792d89cc --- /dev/null +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -0,0 +1,81 @@ +from pyrois.Service_Application_IF import Service_Application_IF +from pyrois import RoIS_Service, RoIS_HRI +from utilities import setup_multi_robot +from pyrois.RoIS_Common import Component_Status +from pyrois.RoIS_Service import Completed_Status + +import logging +import queue +import threading +import time +import enum +import xmlrpc +from termcolor import colored +from transitions import Machine + + +class Task(enum.Enum): + Init = 'Init' + NaviToUser = 'NaviToUser' + NaviToGuide = 'NaviToGuide' + End = 'End' + + +class Service(Service_Application_IF): + def __init__(self, uri="http://localhost:8000", logger=None): + super().__init__(uri) + # buid state machine + states = [Task.Init, Task.NaviToUser, Task.NaviToGuide, Task.End] + transitions = [ + {'trigger': 'StartService', 'source': Task.Init, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'NaviSucceeded', 'source': Task.NaviToUser, 'dest': Task.NaviToGuide, 'after': 'start_navi_to_guide'}, + {'trigger': 'NaviFailed', 'source': Task.NaviToUser, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'NaviSucceeded', 'source': Task.NaviToGuide, 'dest': Task.End, 'after': 'end_service' }, + ] + self.target_pos = "point_c" + self.dest_pos = "point_a" + self.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init, send_event=True) + + def completed(self, command_id, completed_status): + completed_status = Completed_Status(completed_status) + if completed_status == Completed_Status.OK: + self.NaviSucceeded(self.dest_pos) + elif completed_status == Completed_Status.ABORT: + self.NaviFailed(self.target_pos) + + def notify_error(self, error_id, error_type): + print('received error event {}({}) '.format( \ + error_id, RoIS_Service.ErrorType(error_type).name)) + + def notify_event(self, event_id, event_type, subscribe_id, expire): + print('received event {} {} {} {}'.format( \ + event_id, event_type, subscribe_id, expire)) + + def start_navi_to_user(self, event): + dest = event.args[0] + print(colored("NaviToUser started.", 'cyan')) + (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") + stautus = RoIS_HRI.ReturnCode_t(return_code) + + def start_navi_to_guide(self, event): + dest = event.args[0] + print(colored("NaviToGuide started.", 'cyan')) + (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") + stautus = RoIS_HRI.ReturnCode_t(return_code) + + def end_service(self, event): + print(colored("Route Guidance Completed !", 'cyan')) + + def run(self): + self._proxy = xmlrpc.client.ServerProxy(self._uri) + time.sleep(3) + self.StartService(self.target_pos) + + +if __name__ == '__main__': + process = setup_multi_robot() + time.sleep(5) + service = Service() + print("Starting service..") + service.run() + print("Finish.") diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index e9215bf1c3c74e1bc7c4891fb9dcb23dfca12c28..28ddb0094fe6fe6aea5cf7e87268b5f06464f9e8 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -169,6 +169,7 @@ class EventIF(RoIS_HRI.EventIF): from pyrois.Service_Application_Base_example import Service_Application_Base from Navigation_client import Navigation_Client as NavClient from System_Information_client import System_Information_Client as SysClient +from Dummy_Speech_Recognition_client import Speech_Recognition_Client as SRecogClient from pyrois.RoIS_Common import Component_Status import queue import time @@ -182,7 +183,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.event_queue = queue.Queue() self.compoent_clients = { 'Navigation': NavClient('http://localhost:' + str(engine_port+1)), - 'System_Information': SysClient('http://localhost:' + str(engine_port+2)) + 'System_Information': SysClient('http://localhost:' + str(engine_port+2)), + 'Speech_Recognition': SRecogClient('http://localhost:' + str(engine_port+3), self.event_queue) } self.command_id_status_table = {} @@ -205,6 +207,13 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): result = target_component_client.robot_position() result = (result[0].value, result[1], result[2], result[3]) # FIXME self.command_id_status_table[str(self.command_id)] = result + elif component_ref == 'Speech_Recognition': + status = target_component_client.set_parameter(*parameters).value + th = threading.Thread( + target=self.analyze_c_status, + daemon=True, + args=(target_component_client, str(self.command_id))) + th.start() return (status, str(self.command_id)) def get_command_result(self, command_id, condition): diff --git a/route_guidance_ros/scripts/test_app.py b/route_guidance_ros/scripts/test_app.py deleted file mode 100644 index 4e33277f65f672837f74917fd91a4212e0faba70..0000000000000000000000000000000000000000 --- a/route_guidance_ros/scripts/test_app.py +++ /dev/null @@ -1,23 +0,0 @@ -# 1. launch multi navigation. -# 2. run this test. - - -from service_app import Service_Application_IF as Service -import time -from pyrois.RoIS_Common import Component_Status -from utilities import setup_multi_robot - -process = setup_multi_robot() - -time.sleep(10) -print("launching Service...") -service = Service('http://localhost:8000') -print("Service was established.") -for dest in [[25, 0, 0], [0, 20, 0], [5, 0, 0]]: - status = service.go_robot_to(dest) - print(status) - time.sleep(5) -# service.all_finished() - -time.sleep(30) -print("finish") diff --git a/route_guidance_ros/scripts/test_sr_component.py b/route_guidance_ros/scripts/test_sr_component.py new file mode 100644 index 0000000000000000000000000000000000000000..f42c3e2fbf6d616840be488df0f3d826b817b0af --- /dev/null +++ b/route_guidance_ros/scripts/test_sr_component.py @@ -0,0 +1,13 @@ +# Test script for Speech_Recognition + +# 1. run Dummy_Speech_Recognition.py. +# 2. run this test. + + +from Dummy_Speech_Recognition_client import Speech_Recognition_Client +import time + + +uri = "http://localhost:8042" +client = Speech_Recognition_Client(uri) +time.sleep(10) diff --git a/route_guidance_ros/scripts/test_sub_engine_with_service.py b/route_guidance_ros/scripts/test_sub_engine_with_service.py index a0e9d53db73a7c1cf48322845a364a1e106a8672..ca61e696a1dc2f1cb001ffbdb843d56239704168 100644 --- a/route_guidance_ros/scripts/test_sub_engine_with_service.py +++ b/route_guidance_ros/scripts/test_sub_engine_with_service.py @@ -44,12 +44,24 @@ class Service(Service_Application_IF): print("commmand_status:\n", self.commmand_status_table) print("----------------------------------------------") + def recog_speech(self): + print("Starting recog_speech...") + time.sleep(3) + self._proxy = xmlrpc.client.ServerProxy(self._uri) + time.sleep(3) + status, command_id = \ + self._proxy.set_parameter('Speech_Recognition', "lang", "grammer", "rule") + self.commmand_status_table[command_id] = CommandStatus(False) + print("----------------------------------------------") + print("commmand_status:\n", self.commmand_status_table) + print("----------------------------------------------") + + def run(self): destinations = ["point_e", "point_d", "point_c", "point_b"] for dest in destinations: - self.go_robot_to(dest) - while self.is_navigation_running: - time.sleep(1) + self.recog_speech() + time.sleep(3) process = setup_single_robot() diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py index 0802b5e43b7db97165fbddff7ce1e4b35920e81b..75a600dd81262e131a7d8dca641af039487b96e4 100644 --- a/route_guidance_ros/scripts/utilities.py +++ b/route_guidance_ros/scripts/utilities.py @@ -34,6 +34,11 @@ class SysInfoComponent(ComponentWrapper): super().__init__("System_Information.py", robot_name, port) +class SpeechRecogComponent(SubprocessWrapper): + def __init__(self, port): + super().__init__(["python3", "Dummy_Speech_Recognition.py", str(port)]) + + class SubEngineWrapper(SubprocessWrapper): def __init__(self, engine_port): super().__init__(["python3", "sub_engine.py", str(engine_port)]) @@ -49,6 +54,7 @@ def launch_components_and_subengines(**robot_port_table): for robot_name, engine_port in robot_port_table.items(): process.append(NavigationComponent(robot_name, engine_port+1)) process.append(SysInfoComponent(robot_name, engine_port+2)) + process.append(SpeechRecogComponent(engine_port+3)) print("Components Constructed.") time.sleep(5)