diff --git a/rois_description/robot/human1.urdf.xacro b/rois_description/robot/human1.urdf.xacro index d367df1ed7873d3c9942fcf0f21eed7e49a0f6ce..f4dc50a66758637480f87cd1a608268ed8835746 100644 --- a/rois_description/robot/human1.urdf.xacro +++ b/rois_description/robot/human1.urdf.xacro @@ -6,6 +6,7 @@ + @@ -16,7 +17,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 + + + + diff --git a/rois_gazebo/config/human1_controller.yaml b/rois_gazebo/config/human1_controller.yaml index ddf31bf486d4d09bde15875bda27a110713fcd34..45ae02e5c2c7e20e7b58ef0776d8fe87acf830df 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 diff --git a/rois_gazebo/launch/spawn_human.launch b/rois_gazebo/launch/spawn_human.launch index 98863fe865b6ada7b6296d6649f6f345fd3315e1..bb15056e6380686157a1f34ab8672f0f0e8682b7 100644 --- a/rois_gazebo/launch/spawn_human.launch +++ b/rois_gazebo/launch/spawn_human.launch @@ -12,4 +12,12 @@ + + + + + + + + diff --git a/route_guidance_ros/scripts/Person_Localization.py b/route_guidance_ros/scripts/Person_Localization.py new file mode 100755 index 0000000000000000000000000000000000000000..31134e2da8927a4ae13c981584e9adf3a05bc8cc --- /dev/null +++ b/route_guidance_ros/scripts/Person_Localization.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python + +# Person_Localization.py + +"""Person_Localization +""" + +import sys +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 + +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) + + +import rospy +from geometry_msgs.msg import PoseWithCovarianceStamped + +class Person_Localization(Event, Command, Query): + """Person_Localization + """ + 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) + + +class component: + """component + """ + def __init__(self): + self._state = False + + +def example_pl(port, person_name): + """example_pl + """ + c = component() + pl = Person_Localization(c, person_name) + + # 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__': + 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) 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 0000000000000000000000000000000000000000..aac1cb321e3fd8aee4a7688c8c76de8473044b3f --- /dev/null +++ b/route_guidance_ros/scripts/Person_Localization_client.py @@ -0,0 +1,107 @@ +# 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_Localization_Client(Command, Query, Event): + """Person_localizaion_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() + 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 09b639c084705a1547fc0213e0bc790f088b5dff..5ef2f018c4475ae133c8953379733ddc3ee5d42b 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 Dummy_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 @@ -192,6 +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', self.component_event_queue) } self.engine_clients = { 'robot1': EngineClient('http://localhost:8010'), @@ -267,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) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 05ed24cb9b8f8b4d7835a6438593a34f759dfe79..344d0967655ec5dad9804293cffaaefada1d839d 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,15 +27,15 @@ 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' }, {'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 = {} @@ -56,9 +57,21 @@ 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 + 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) diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py index 8554eedbdb69bf981af290065e0e7a78469d8cc9..cbfd97b0e1922137fb4ba398d5bfdc3e5a7e0b9d 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.")