From 4355cbe69bf95707434459cf94017e40af5c6e29 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 15:00:56 +0900 Subject: [PATCH 01/16] Add sample Person_Localization component --- .../scripts/Person_Localization.py | 129 ++++++++++++++++++ .../scripts/Person_Localization_client.py | 100 ++++++++++++++ 2 files changed, 229 insertions(+) create mode 100755 route_guidance_ros/scripts/Person_Localization.py create mode 100644 route_guidance_ros/scripts/Person_Localization_client.py diff --git a/route_guidance_ros/scripts/Person_Localization.py b/route_guidance_ros/scripts/Person_Localization.py new file mode 100755 index 00000000..fb025826 --- /dev/null +++ b/route_guidance_ros/scripts/Person_Localization.py @@ -0,0 +1,129 @@ +# Person_Localization.py + +"""Person_Detection +""" + +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, detection_threshold, minimum_interval): + self._component.Detection_Threshold = detection_threshold + self._component.Minimum_Interval = minimum_interval + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, c): + self._component = c + # self._component.Detection_Threshold = 1000 + # self._component.Minimum_Interval = 20 + + 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.Detection_Threshold, self._component.Minimum_Interval) + + +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 person_localized(self, timestamp, person_ref, position_data): + """person_localization + """ + msg = xmlrpc.client.dumps((timestamp, person_ref, position_data), 'person_localized') + self.event_queue.put(msg) + + +class Person_Localization(Event, Command, Query): + """Person_Localization + """ + def __init__(self, c): + super().__init__(c) + self._component = c + self._component.Detection_Threshold = 1000 + self._component.Minimum_Interval = 20 + + +class component: + """component + """ + def __init__(self): + self._state = False + + +def example_pl(port): + """example_pd + """ + c = component() + pl = Person_Localization(c) + + # start the XML-RPC server + server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) + server.register_instance(pl) + server.register_introspection_functions() + server.register_multicall_functions() + server.serve_forever() + + +if __name__ == '__main__': + example_pl(8003) diff --git a/route_guidance_ros/scripts/Person_Localization_client.py b/route_guidance_ros/scripts/Person_Localization_client.py new file mode 100644 index 00000000..1bf73ee7 --- /dev/null +++ b/route_guidance_ros/scripts/Person_Localization_client.py @@ -0,0 +1,100 @@ +# Person_Localization_Client.py + +"""Person_Localization_Client +""" + +import threading +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, detection_threshold, minimum_interval): + s = self._proxy.set_parameter(detection_threshold, minimum_interval) + 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, detection_threshold, minimum_interval) = self._proxy.get_parameter() + return (RoIS_HRI.ReturnCode_t(status), detection_threshold, minimum_interval) + + +class Event(RoIS_Common.Event): + """Event + """ + def __init__(self, uri): + self._uri = uri + self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + self.events = [] + + 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) + print(params,methodname) + if methodname == 'person_localized': + self.person_localized(params[0], params[1], params[2]) + + def person_localized(self, timestamp, person_ref, position_data): + self.events.append((timestamp,person_ref,position_data)) + print("person_localized",timestamp, person_ref, position_data) + + +class Person_Localizaion_Client(Command, Query, Event): + """Person_localizaion_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 3adcd0c3d48d50f5738c301211452783dcb13903 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 15:05:17 +0900 Subject: [PATCH 02/16] Fix Person_Localization server for python2 --- route_guidance_ros/scripts/Person_Localization.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/Person_Localization.py b/route_guidance_ros/scripts/Person_Localization.py index fb025826..df453781 100755 --- a/route_guidance_ros/scripts/Person_Localization.py +++ b/route_guidance_ros/scripts/Person_Localization.py @@ -4,7 +4,7 @@ """ import sys -import queue +from six.moves import queue import time from datetime import datetime import threading @@ -98,7 +98,7 @@ class Person_Localization(Event, Command, Query): """Person_Localization """ def __init__(self, c): - super().__init__(c) + super(Person_Localization, self).__init__(c) self._component = c self._component.Detection_Threshold = 1000 self._component.Minimum_Interval = 20 -- GitLab From 1df3de82321629b77b1f680313125d69c8bd4972 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 15:24:32 +0900 Subject: [PATCH 03/16] Upgrade Person_Localization as ROS node --- .../scripts/Person_Localization.py | 31 +++++++++++++++---- 1 file changed, 25 insertions(+), 6 deletions(-) diff --git a/route_guidance_ros/scripts/Person_Localization.py b/route_guidance_ros/scripts/Person_Localization.py index df453781..298b37b8 100755 --- a/route_guidance_ros/scripts/Person_Localization.py +++ b/route_guidance_ros/scripts/Person_Localization.py @@ -1,6 +1,8 @@ +#!/usr/bin/env python + # Person_Localization.py -"""Person_Detection +"""Person_Localization """ import sys @@ -94,14 +96,25 @@ class Event(RoIS_Common.Event): self.event_queue.put(msg) +import rospy +from geometry_msgs.msg import PoseWithCovarianceStamped + class Person_Localization(Event, Command, Query): """Person_Localization """ - def __init__(self, c): + def __init__(self, c, person_name): super(Person_Localization, self).__init__(c) self._component = c self._component.Detection_Threshold = 1000 self._component.Minimum_Interval = 20 + self.person_ref = person_name + rospy.Subscriber(person_name+'/amcl_pose', PoseWithCovarianceStamped, self.amcl_pose_callback) + self.position = None + + def amcl_pose_callback(self, amcl_pose): + pos = amcl_pose.pose.pose.position + self.position = [pos.x, pos.y, 0.0] + self.person_localized(datetime.now(), self.person_ref, self.position_data) class component: @@ -111,11 +124,11 @@ class component: self._state = False -def example_pl(port): - """example_pd +def example_pl(port, person_name): + """example_pl """ c = component() - pl = Person_Localization(c) + pl = Person_Localization(c, person_name) # start the XML-RPC server server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) @@ -126,4 +139,10 @@ def example_pl(port): if __name__ == '__main__': - example_pl(8003) + if len(sys.argv) < 3: + print("rosrun ") + raise RuntimeError + + port, person_name = int(sys.argv[1]), sys.argv[2] + rospy.init_node('person_localization', anonymous=True) + example_pl(port, person_name) -- GitLab From 492b42fd518004c7d68993fa9975c0126eaebdf3 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 15:56:10 +0900 Subject: [PATCH 04/16] Launch Person_Localization server --- route_guidance_ros/scripts/utilities.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py index 789eeeca..38cd389b 100644 --- a/route_guidance_ros/scripts/utilities.py +++ b/route_guidance_ros/scripts/utilities.py @@ -58,6 +58,13 @@ def launch_components_and_subengines(**robot_port_table): print("Components Constructed.") time.sleep(5) + command = [ + "rosrun", "route_guidance_ros", "Person_Localization.py", + "8043", "human1" + ] + process.append(SubprocessWrapper(command)) + print("Person Localization Component Constructed.") + for engine_port in robot_port_table.values(): process.append(SubEngineWrapper(engine_port)) print("Sub Engine Constructed.") @@ -84,6 +91,12 @@ def setup_multi_robot(): ] process.append(SubprocessWrapper(command)) print("Virtual Speech-Recognition Component Constructed.") + command = [ + "rosrun", "route_guidance_ros", "Person_Localization.py", + "8043", "human1" + ] + process.append(SubprocessWrapper(command)) + print("Person Localization Component Constructed.") time.sleep(5) process.append(MainEngineWrapper()) print("Main Engine Constructed.") -- GitLab From c3f5d564e8d910a0ffd477dd480e04c43c674254 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 15:56:56 +0900 Subject: [PATCH 05/16] Fix typo on Person_Localization_client --- route_guidance_ros/scripts/Person_Localization_client.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/Person_Localization_client.py b/route_guidance_ros/scripts/Person_Localization_client.py index 1bf73ee7..8cccf04d 100644 --- a/route_guidance_ros/scripts/Person_Localization_client.py +++ b/route_guidance_ros/scripts/Person_Localization_client.py @@ -89,7 +89,7 @@ class Event(RoIS_Common.Event): print("person_localized",timestamp, person_ref, position_data) -class Person_Localizaion_Client(Command, Query, Event): +class Person_Localization_Client(Command, Query, Event): """Person_localizaion_Client """ def __init__(self, uri): -- GitLab From d988ea0962540ff6202db558197da4fb91c86007 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 16:02:52 +0900 Subject: [PATCH 06/16] Add config for LRF model to human model --- rois_description/robot/human1.urdf.xacro | 49 ++++++++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/rois_description/robot/human1.urdf.xacro b/rois_description/robot/human1.urdf.xacro index d367df1e..a49cab14 100644 --- a/rois_description/robot/human1.urdf.xacro +++ b/rois_description/robot/human1.urdf.xacro @@ -6,6 +6,7 @@ + @@ -147,6 +148,54 @@ + + + + + + + + + + + + + + + + + + + Gazebo/Blue + false + + 0.2 0 0 0 0 0 + false + 40 + + + + 500 + 1 + -${M_PI*3/4} + ${M_PI*3/4} + + + + 0.20 + 131 + 0.01 + + + + scan + lrf_link + 0.9 + 130 + + + + -- GitLab From 7822c723d3803554af6d2c0bc9363f0a697c046b Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 16:12:32 +0900 Subject: [PATCH 07/16] Add amcl node config to spawn_human launcher --- rois_gazebo/launch/spawn_human.launch | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/rois_gazebo/launch/spawn_human.launch b/rois_gazebo/launch/spawn_human.launch index 98863fe8..bb15056e 100644 --- a/rois_gazebo/launch/spawn_human.launch +++ b/rois_gazebo/launch/spawn_human.launch @@ -12,4 +12,12 @@ + + + + + + + + -- GitLab From f444ee3892a3036fe9002390ece56864c17ef86a Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 16:21:30 +0900 Subject: [PATCH 08/16] Fix runtime error on Person_Localization server --- route_guidance_ros/scripts/Person_Localization.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/Person_Localization.py b/route_guidance_ros/scripts/Person_Localization.py index 298b37b8..31134e2d 100755 --- a/route_guidance_ros/scripts/Person_Localization.py +++ b/route_guidance_ros/scripts/Person_Localization.py @@ -10,6 +10,7 @@ from six.moves import queue import time from datetime import datetime import threading +from six.moves import xmlrpc_client from pyrois import RoIS_Common, RoIS_HRI @@ -92,7 +93,7 @@ class Event(RoIS_Common.Event): def person_localized(self, timestamp, person_ref, position_data): """person_localization """ - msg = xmlrpc.client.dumps((timestamp, person_ref, position_data), 'person_localized') + msg = xmlrpc_client.dumps((timestamp, person_ref, position_data), 'person_localized') self.event_queue.put(msg) @@ -114,7 +115,7 @@ class Person_Localization(Event, Command, Query): def amcl_pose_callback(self, amcl_pose): pos = amcl_pose.pose.pose.position self.position = [pos.x, pos.y, 0.0] - self.person_localized(datetime.now(), self.person_ref, self.position_data) + self.person_localized(datetime.now(), self.person_ref, self.position) class component: -- GitLab From 076ee86d434353195fdf608795261ff1f6105a6c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 16:22:17 +0900 Subject: [PATCH 09/16] Add Person_Localization client on main_engine --- route_guidance_ros/scripts/main_engine.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index a14ae7db..0959a066 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -170,6 +170,7 @@ class EventIF(RoIS_HRI.EventIF): from VirtualNavigation_client import VirtualNavigation_Client as VNavClient from Virtual_Speech_Recognition_client import Speech_Recognition_Client as SRecogClient +from Person_Localization_client import Person_Localization_Client as PLClient from sub_engine_client import IF as EngineClient from pyrois.RoIS_Common import Component_Status from pyrois.RoIS_Service import Completed_Status @@ -186,7 +187,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.component_event_queue = queue.Queue() self.component_clients = { 'Navigation': VNavClient('http://localhost:8041', self.component_event_queue), - 'Speech_Recognition': SRecogClient('http://localhost:8042', self.component_event_queue) + 'Speech_Recognition': SRecogClient('http://localhost:8042', self.component_event_queue), + 'Person_Localization': PLClient('http://localhost:8043') } self.engine_clients = { 'robot1': EngineClient('http://localhost:8010'), -- GitLab From 4e8b541902fbd461387ade56ab08891cd2be3021 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 16:55:51 +0900 Subject: [PATCH 10/16] Remove unnecessary process of Person_Localization --- route_guidance_ros/scripts/utilities.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py index 38cd389b..a1d5188b 100644 --- a/route_guidance_ros/scripts/utilities.py +++ b/route_guidance_ros/scripts/utilities.py @@ -91,12 +91,6 @@ def setup_multi_robot(): ] process.append(SubprocessWrapper(command)) print("Virtual Speech-Recognition Component Constructed.") - command = [ - "rosrun", "route_guidance_ros", "Person_Localization.py", - "8043", "human1" - ] - process.append(SubprocessWrapper(command)) - print("Person Localization Component Constructed.") time.sleep(5) process.append(MainEngineWrapper()) print("Main Engine Constructed.") -- GitLab From d367061aee896c150e491bd950a720ff2573bbb7 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 29 Jan 2020 14:21:43 +0900 Subject: [PATCH 11/16] Change shape of human model --- rois_description/robot/human1.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rois_description/robot/human1.urdf.xacro b/rois_description/robot/human1.urdf.xacro index a49cab14..f4dc50a6 100644 --- a/rois_description/robot/human1.urdf.xacro +++ b/rois_description/robot/human1.urdf.xacro @@ -17,7 +17,7 @@ - + -- GitLab From faa9ee305cd116d04f0ba4d319e2b30725f8df56 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 29 Jan 2020 14:24:28 +0900 Subject: [PATCH 12/16] Adjust human models runnning speed --- rois_gazebo/config/human1_controller.yaml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/rois_gazebo/config/human1_controller.yaml b/rois_gazebo/config/human1_controller.yaml index ddf31bf4..45ae02e5 100644 --- a/rois_gazebo/config/human1_controller.yaml +++ b/rois_gazebo/config/human1_controller.yaml @@ -31,16 +31,16 @@ diff_drive_controller: linear: x: has_velocity_limits : true - max_velocity : 2.0 # m/s - min_velocity : -2.0 # m/s + max_velocity : 1.0 # m/s + min_velocity : -1.0 # m/s has_acceleration_limits: true - max_acceleration : 2.0 # m/s^2 - min_acceleration : -2.0 # m/s^2 + max_acceleration : 0.5 # m/s^2 + min_acceleration : -0.5 # m/s^2 angular: z: has_velocity_limits : true - max_velocity : 1.0 # rad/s - min_velocity : -1.0 + max_velocity : 0.5 # rad/s + min_velocity : -0.5 has_acceleration_limits: true - max_acceleration : 1.0 # rad/s^2 - min_acceleration : -1.0 + max_acceleration : 0.5 # rad/s^2 + min_acceleration : -0.5 -- GitLab From de73e8f066b9b75e8e455ca29bcee0157a25e47c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 30 Jan 2020 00:06:57 +0900 Subject: [PATCH 13/16] Add engine_event_queue to Person_Localization_client --- route_guidance_ros/scripts/Person_Localization_client.py | 9 ++++++++- route_guidance_ros/scripts/main_engine.py | 2 +- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/Person_Localization_client.py b/route_guidance_ros/scripts/Person_Localization_client.py index 8cccf04d..aac1cb32 100644 --- a/route_guidance_ros/scripts/Person_Localization_client.py +++ b/route_guidance_ros/scripts/Person_Localization_client.py @@ -92,9 +92,16 @@ class Event(RoIS_Common.Event): class Person_Localization_Client(Command, Query, Event): """Person_localizaion_Client """ - def __init__(self, uri): + 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() + self.engine_event_queue.put(msg) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 2bcf0106..dbf2972d 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -193,7 +193,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): 'Speech_Recognition1': SRecogClient('http://localhost:8013', self.component_event_queue), 'Speech_Recognition2': SRecogClient('http://localhost:8023', self.component_event_queue), 'Speech_Recognition3': SRecogClient('http://localhost:8033', self.component_event_queue), - 'Person_Localization': PLClient('http://localhost:8043') + 'Person_Localization': PLClient('http://localhost:8043', self.component_event_queue) } self.engine_clients = { 'robot1': EngineClient('http://localhost:8010'), -- GitLab From 55bfccd72232c644b03f6bd5fc8a150584bc5348 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 30 Jan 2020 00:14:56 +0900 Subject: [PATCH 14/16] Handle person_localized event on MainEngine::monitor_event --- route_guidance_ros/scripts/main_engine.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index dbf2972d..5ef2f018 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -269,10 +269,19 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): try: subscribed_id = self.event_ref_subscribe_id_table['speech_recognized'+target_robot] except KeyError: - print("This is un-subscribed event.") + print("This is un-subscribed event. (speech_recognized)") continue self.event_id_result_table[event_id] = params self.notify_event(event_id, 'speech_recognized', subscribed_id, "") + elif methodname == 'person_localized': + timestamp, person_ref, position_data = params + try: + subscribed_id = self.event_ref_subscribe_id_table['person_localized'] + except KeyError: + print("This is un-subscribed event. (person_localized)") + continue + self.event_id_result_table[event_id] = params + self.notify_event(event_id, 'person_localized', subscribed_id, "") else: raise RuntimeError("[main_engine]Unknown method-name of event.") time.sleep(3) -- GitLab From bafbba586d5ea519d9e310e28d03fd706bb63965 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 30 Jan 2020 00:28:14 +0900 Subject: [PATCH 15/16] Add GetUserPos task on service_app --- route_guidance_ros/scripts/service_app_lv2.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 05ed24cb..bc36fce9 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -16,6 +16,7 @@ from transitions import Machine class Task(enum.Enum): Init = 'Init' + GetUserPos = 'GetUserPos' NaviToUser = 'NaviToUser' ListenDest = 'ListenDest' NaviToGuide = 'NaviToGuide' @@ -26,9 +27,10 @@ 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.ListenDest, Task.NaviToGuide, Task.End] + states = [Task.Init, Task.GetUserPos, Task.NaviToUser, Task.ListenDest, Task.NaviToGuide, Task.End] transitions = [ - {'trigger': 'Start', 'source': Task.Init, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'Start', 'source': Task.Init, 'dest': Task.GetUserPos, 'after': 'get_user_position' }, + {'trigger': 'Succeeded', 'source': Task.GetUserPos, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, {'trigger': 'Succeeded', 'source': Task.NaviToUser, 'dest': Task.ListenDest, 'after': 'listen_destination' }, {'trigger': 'Succeeded', 'source': Task.ListenDest, 'dest': Task.NaviToGuide, 'after': 'start_navi_to_guide'}, {'trigger': 'Failed', 'source': Task.NaviToUser, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, @@ -56,6 +58,15 @@ class Service(Service_Application_IF): print(colored("speech recognized successfully.", 'green')) return_code = self._proxy.unsubscribe(subscribe_id) self.Succeeded() + if event_type == 'person_localized' and self.state == Task.GetUserPos: + self.latest_event_id_table['person_localized'] = event_id + print(colored("person localized successfully.", 'green')) + return_code = self._proxy.unsubscribe(subscribe_id) + self.Succeeded() + + def get_user_position(self): + return_code, subscribe_id = self._proxy.subscribe('person_localized', "") + print(colored("GetUserPos started.", 'cyan')) def start_navi_to_user(self): dest = self.target_pos -- GitLab From 403aedfaddb1dbf8d7d857e642d8fc49a12a438e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 30 Jan 2020 00:57:12 +0900 Subject: [PATCH 16/16] Use result of person_localization --- route_guidance_ros/scripts/service_app_lv2.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index bc36fce9..344d0967 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -36,7 +36,6 @@ class Service(Service_Application_IF): {'trigger': 'Failed', 'source': Task.NaviToUser, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, {'trigger': 'Succeeded', 'source': Task.NaviToGuide, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, ] - self.target_pos = "point_c" self.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init) self.latest_event_id_table = {} @@ -69,7 +68,10 @@ class Service(Service_Application_IF): print(colored("GetUserPos started.", 'cyan')) def start_navi_to_user(self): - dest = self.target_pos + pl_event_id = self.latest_event_id_table['person_localized'] + status, results = self._proxy.get_event_detail(pl_event_id, "") + dest = results[2] + print("dest is ", dest) print(colored("NaviToUser started.", 'cyan')) (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") stautus = RoIS_HRI.ReturnCode_t(return_code) -- GitLab