From 523056956f77f030c8522fbd1223c897892e1c8f Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 17:27:35 +0900 Subject: [PATCH 1/7] Add VirtualNavigation component --- goal_sender/scripts/VirtualNavigation.py | 156 +++++++++++++++++++++++ 1 file changed, 156 insertions(+) create mode 100755 goal_sender/scripts/VirtualNavigation.py diff --git a/goal_sender/scripts/VirtualNavigation.py b/goal_sender/scripts/VirtualNavigation.py new file mode 100755 index 00000000..d7ba78f9 --- /dev/null +++ b/goal_sender/scripts/VirtualNavigation.py @@ -0,0 +1,156 @@ +#!/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 + +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, + lambda amcl_pose: self.update_position(robot_name, amcl_pose)) + + + 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) + self.__goal_senders[nearest_robot].send_goal(dest) + + +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('navigation_component', anonymous=True) + example_n(port=port, robot_names=robot_names) -- GitLab From 546ef463e86f26bbe5d80ec2e7f2cb8b9e9b588a Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 17:41:17 +0900 Subject: [PATCH 2/7] Add client of VirtualNavigation component --- .../scripts/VirtualNavigation_client.py | 107 ++++++++++++++++++ 1 file changed, 107 insertions(+) create mode 100644 goal_sender/scripts/VirtualNavigation_client.py diff --git a/goal_sender/scripts/VirtualNavigation_client.py b/goal_sender/scripts/VirtualNavigation_client.py new file mode 100644 index 00000000..be14ab33 --- /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() -- GitLab From 062c880db38e5fee101668a884823ac8080e9d40 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 17:48:09 +0900 Subject: [PATCH 3/7] Add sample of set_parameter utilizing virtual component --- goal_sender/scripts/toplevel_route_guidance_engine.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/goal_sender/scripts/toplevel_route_guidance_engine.py b/goal_sender/scripts/toplevel_route_guidance_engine.py index aa5aa1e7..8dcb68c6 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,11 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): self.command_id += 1 return (status, str(self.command_id)) + def set_parameter2(self, component_ref, parameters): + 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 -- GitLab From 9a17703bb972a8d2c3428dff09e46a18eb62a19e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 18:27:16 +0900 Subject: [PATCH 4/7] Fix VirtualNavigation component's set_parameter --- goal_sender/scripts/VirtualNavigation.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/goal_sender/scripts/VirtualNavigation.py b/goal_sender/scripts/VirtualNavigation.py index d7ba78f9..056888be 100755 --- a/goal_sender/scripts/VirtualNavigation.py +++ b/goal_sender/scripts/VirtualNavigation.py @@ -103,6 +103,8 @@ 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 @@ -111,8 +113,8 @@ class Component(object): 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, - lambda amcl_pose: self.update_position(robot_name, amcl_pose)) + PoseWithCovarianceStamped, + partial(self.update_position, robot_name)) def update_position(self, robot_name, amcl_pose): @@ -125,7 +127,8 @@ class Component(object): 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) - self.__goal_senders[nearest_robot].send_goal(dest) + print("nearest_robot is ", nearest_robot) + self.__goal_senders[nearest_robot].send_goal(dest.tolist()) def example_n(port, robot_names): @@ -152,5 +155,5 @@ if __name__ == '__main__': raise RuntimeError port, robot_names = int(sys.argv[1]), sys.argv[2:] - rospy.init_node('navigation_component', anonymous=True) + rospy.init_node('virtual_navigation_component', anonymous=True) example_n(port=port, robot_names=robot_names) -- GitLab From 11e852afe8fbcc3e57209355f07a8ee34904258e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 18:28:52 +0900 Subject: [PATCH 5/7] Add debug code for virtual-component --- goal_sender/scripts/toplevel_route_guidance_engine.py | 1 + 1 file changed, 1 insertion(+) diff --git a/goal_sender/scripts/toplevel_route_guidance_engine.py b/goal_sender/scripts/toplevel_route_guidance_engine.py index 8dcb68c6..00464d04 100644 --- a/goal_sender/scripts/toplevel_route_guidance_engine.py +++ b/goal_sender/scripts/toplevel_route_guidance_engine.py @@ -202,6 +202,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): 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 -- GitLab From 6ea176fc2004d24ef5490fb2cd1dd7cad98e5ee8 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 18:29:25 +0900 Subject: [PATCH 6/7] Add client's function to call set_parameter2 --- .../scripts/toplevel_route_guidance_engine_client.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/goal_sender/scripts/toplevel_route_guidance_engine_client.py b/goal_sender/scripts/toplevel_route_guidance_engine_client.py index b522efb7..760af94a 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) -- GitLab From aeee384cff57cdfb1a6ed74743348e886c29385f Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 18:31:27 +0900 Subject: [PATCH 7/7] Add test for VirtualNavigation component --- goal_sender/scripts/test_vnav_component.py | 33 ++++++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 goal_sender/scripts/test_vnav_component.py diff --git a/goal_sender/scripts/test_vnav_component.py b/goal_sender/scripts/test_vnav_component.py new file mode 100644 index 00000000..e0b9c89f --- /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") -- GitLab