diff --git a/goal_sender/scripts/VirtualNavigation.py b/goal_sender/scripts/VirtualNavigation.py new file mode 100755 index 0000000000000000000000000000000000000000..056888befdfc476aed8163abce0c114cf859146a --- /dev/null +++ b/goal_sender/scripts/VirtualNavigation.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python + +# VirtualNavigation.py +# +# Copyright 2018 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python3 +# For HRI Component + +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 Queue as queue + import SocketServer + import SimpleXMLRPCServer + + class ThreadingXMLRPCServer(SocketServer.ThreadingMixIn, SimpleXMLRPCServer.SimpleXMLRPCServer): + pass +else: + import queue + import socketserver + import xmlrpc.server + + class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer): + pass + + +class Command(RoIS_Common.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 + + +class Query(RoIS_Common.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) + + +class Event(RoIS_Common.Event): + def __init__(self, c): + self._component = c + self.event_queue = queue.Queue() + + # def poll_event(self): + # msg = self.event_queue.get() + # return msg + +from goal_sender_ros import GoalSenderROS + +class VirtualNavigation(Event, Command, Query): + def __init__(self, c, robot_names): + super(VirtualNavigation, self).__init__(c) + self._component = c + self._component.Target_Position = [""] + self._component.Time_Limit = 10 + self._component.Routing_Policy = "" + + def set_parameter(self, target_position, time_limit, routing_policy): + self._component.send_goal_to_nearest_robot(target_position) + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def component_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + c_status = self._goal_sender.get_goalsender_state().value + return (status, c_status) + + +def event_dispatch(n): + # n.person_detected(datetime.now().isoformat(), 1) + time.sleep(0.1) + # n.person_detected(datetime.now().isoformat(), 1) + +import numpy as np +import rospy +from geometry_msgs.msg import PoseWithCovarianceStamped + +from functools import partial + +class Component(object): + def __init__(self, robot_names): + self.__robot_names = robot_names + self.__goal_senders = \ + {robot_name : GoalSenderROS(robot_name) for robot_name in robot_names} + self.__robot_positions = {robot_name : None for robot_name in robot_names} + for robot_name in robot_names: + rospy.Subscriber(robot_name+"/amcl_pose", + PoseWithCovarianceStamped, + partial(self.update_position, robot_name)) + + + def update_position(self, robot_name, amcl_pose): + pos = amcl_pose.pose.pose.position + self.__robot_positions[robot_name] = np.array([pos.x, pos.y]) + + + def send_goal_to_nearest_robot(self, dest): + dest = np.array(dest) + distance_table = \ + {name: np.linalg.norm(dest - pos) for name, pos in self.__robot_positions.items()} + nearest_robot = min(distance_table, key=distance_table.get) + print("nearest_robot is ", nearest_robot) + self.__goal_senders[nearest_robot].send_goal(dest.tolist()) + + +def example_n(port, robot_names): + print("Starting node...") + component = Component(robot_names) + n = VirtualNavigation(component, robot_names) + print("VirtualNavigation constructed") + + # start the timer to dispatch events + t = threading.Timer(0.1, event_dispatch, args=(n,)) + t.start() + + # start the XML-RPC server + server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) + server.register_instance(n) + server.register_introspection_functions() + server.register_multicall_functions() + server.serve_forever() + + +if __name__ == '__main__': + if len(sys.argv) < 3: + print("rosrun VirtualNavigation.py ...") + raise RuntimeError + + port, robot_names = int(sys.argv[1]), sys.argv[2:] + rospy.init_node('virtual_navigation_component', anonymous=True) + example_n(port=port, robot_names=robot_names) diff --git a/goal_sender/scripts/VirtualNavigation_client.py b/goal_sender/scripts/VirtualNavigation_client.py new file mode 100644 index 0000000000000000000000000000000000000000..be14ab33d157a3c6cb13b22fff2fa61e70bf6ed5 --- /dev/null +++ b/goal_sender/scripts/VirtualNavigation_client.py @@ -0,0 +1,107 @@ +# VirtualNavigation_Client.py +# +# Copyright 2018 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python3 +# For HRI Engine + +"""VirtualNavigation_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, target_position, time_limit, routing_policy): + status = self._proxy.set_parameter(target_position, time_limit, routing_policy) + status = RoIS_HRI.ReturnCode_t(status) + 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, target_position, time_limit, routing_policy) = self._proxy.get_parameter() + return (RoIS_HRI.ReturnCode_t(status), target_position, time_limit, routing_policy) + + +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]) + + +class VirtualNavigation_Client(Command, Query, Event): + """VirtualNavigation_Client + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + self.events = [] + # self.start_th() diff --git a/goal_sender/scripts/test_vnav_component.py b/goal_sender/scripts/test_vnav_component.py new file mode 100644 index 0000000000000000000000000000000000000000..e0b9c89fa5681294c28fef5f298056b62c90c609 --- /dev/null +++ b/goal_sender/scripts/test_vnav_component.py @@ -0,0 +1,33 @@ +# 1. launch navigation. +# 2. execute "rosrun goal_sender VirtualNavigation.py 8041 robot1 robot2 robot3". +# 3. run this test. + + +from toplevel_route_guidance_engine_client import IF as Engine +import time +from pyrois.RoIS_Common import Component_Status +from utilities import NavigationComponent, SysInfoComponent +from subprocess import Popen + +robot_port_table = { + 'robot1': 8010, 'robot2': 8020, 'robot3': 8030 +} +engines = [] +for engine_port in robot_port_table.values(): + engines.append(Popen(["python3", "sub_route_guidance_engine.py", str(engine_port)])) +print("Sub Engine Constructed.") +time.sleep(5) + +toplevel_engine_p = Popen(["python3", "toplevel_route_guidance_engine.py"]) +print("Toplevel Engine Constructed.") +time.sleep(5) + +engine = Engine('http://127.0.0.1:8000') +engine.connect() +for dest in [[25, 0], [0, 20], [5, 0]]: + status = engine.set_parameter2('Navigation', [dest, "", ""]) + print(status) + time.sleep(5) + +engine.disconnect() +print("finish") diff --git a/goal_sender/scripts/toplevel_route_guidance_engine.py b/goal_sender/scripts/toplevel_route_guidance_engine.py index aa5aa1e737f49f78f0f95ac7aea84a394bbfbe55..00464d0472dc12554ebe60f4897462b43653b898 100644 --- a/goal_sender/scripts/toplevel_route_guidance_engine.py +++ b/goal_sender/scripts/toplevel_route_guidance_engine.py @@ -165,6 +165,7 @@ class EventIF(RoIS_HRI.EventIF): results = ["None"] return (status, results) +from VirtualNavigation_client import VirtualNavigation_Client as VNavClient from sub_route_guidance_engine_client import IF as EngineClient class IF(SystemIF, CommandIF, QueryIF, EventIF): @@ -173,6 +174,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): def __init__(self, Engine): super().__init__(Engine) self.command_id = 0 + self.component_clients = { + 'Navigation': VNavClient('http://localhost:8041') + } self.engine_clients = { 'robot1': EngineClient('http://localhost:8010'), 'robot2': EngineClient('http://localhost:8020'), @@ -186,7 +190,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): robot_distance_table = {} for name, engine in self.engine_clients.items(): pos = engine.get_position()[3] - print("OK1") pos = np.array(pos) robot_distance_table[name] = np.linalg.norm(dest - pos) nearest_robot = min(robot_distance_table, key=robot_distance_table.get) @@ -198,6 +201,12 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): self.command_id += 1 return (status, str(self.command_id)) + def set_parameter2(self, component_ref, parameters): + print("Using VirtualNavigation") + target_component = self.component_clients[component_ref] + status = target_component.set_parameter(*parameters).value + self.command_id += 1 + return (status, str(self.command_id)) class IF_server: """IF_Server diff --git a/goal_sender/scripts/toplevel_route_guidance_engine_client.py b/goal_sender/scripts/toplevel_route_guidance_engine_client.py index b522efb77f351816dfc7a2cd1e1bce115edef5de..760af94a75ef5a2534c27aa5e6d7436f3722355b 100644 --- a/goal_sender/scripts/toplevel_route_guidance_engine_client.py +++ b/goal_sender/scripts/toplevel_route_guidance_engine_client.py @@ -73,6 +73,13 @@ class CommandIF(RoIS_HRI.CommandIF): status = RoIS_HRI.ReturnCode_t(s) return (status, command_id) + #### REMOVE #### + def set_parameter2(self, component_ref, parameters): + (s, command_id) = self._proxy.set_parameter2(component_ref, parameters) + status = RoIS_HRI.ReturnCode_t(s) + return (status, command_id) + ############### + def execute(self, command_unit_list): s = self._proxy.execute(command_unit_list) status = RoIS_HRI.ReturnCode_t(s)