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.")