From 64ba5b28e1a33414d9bd0b3f1d5bf79496200bee Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Fri, 18 Oct 2019 16:00:08 +0900
Subject: [PATCH 01/20] Add plane HRI-engine for route-guidance

---
 goal_sender/scripts/route_guidance_engine.py | 205 +++++++++++++++++++
 1 file changed, 205 insertions(+)
 create mode 100644 goal_sender/scripts/route_guidance_engine.py

diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py
new file mode 100644
index 00000000..3da9e8e5
--- /dev/null
+++ b/goal_sender/scripts/route_guidance_engine.py
@@ -0,0 +1,205 @@
+# route_guidance_engine.py
+#
+# Copyright 2017 Eiichi Inohira
+# This software may be modified and distributed under the terms
+# of the MIT license
+#
+# For python 3
+# For HRI Engine
+
+"""route_guidance_engine
+"""
+
+from __future__ import print_function
+
+import os
+import sys
+
+from pyrois import RoIS_HRI
+
+if sys.version_info.major == 2:
+    import SocketServer
+    import SimpleXMLRPCServer
+
+    class ThreadingXMLRPCServer(SocketServer.ThreadingMixIn, SimpleXMLRPCServer.SimpleXMLRPCServer):
+        """ThreadingXMLRPCServer
+        """
+        pass
+else:
+    import socketserver
+    import xmlrpc.server
+
+    class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer):
+        """ThreadingXMLRPCServer
+        """
+        pass
+
+
+class SystemIF(RoIS_HRI.SystemIF):
+    """SystemIF
+    """
+    def __init__(self, Engine):
+        self._engine = Engine
+
+    def connect(self):
+        # print("connect")
+        if self._engine.state is False:
+            self._engine.state = True
+            status = RoIS_HRI.ReturnCode_t.OK.value
+        else:
+            status = RoIS_HRI.ReturnCode_t.ERROR.value
+        # time.sleep(30)
+        return status
+
+    def disconnect(self):
+        if self._engine.state is True:
+            self._engine.state = False
+            status = RoIS_HRI.ReturnCode_t.OK.value
+        else:
+            status = RoIS_HRI.ReturnCode_t.ERROR.value
+        #status = RoIS_HRI.ReturnCode_t.OK.value
+        return status
+
+    def get_profile(self, condition):
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        # RoIS_HRI_Profile
+        profile = "Unsupported"
+        return (status, profile)
+
+    def get_error_detail(self, error_id, condition):
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        # ResultLists
+        results = "None"
+        return (status, results)
+
+
+class CommandIF(RoIS_HRI.CommandIF):
+    """CommandIF
+    """
+    def __init__(self, Engine):
+        self._engine = Engine
+
+    def search(self, condition):
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        # List< RoIS_Identifier>
+        component_ref_list = ["None"]
+        return (status, component_ref_list)
+
+    def bind(self, component_ref):
+        # print("state:", self._engine.state)
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        return status
+
+    def bind_any(self, condition):
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        component_ref_list = ["None"]
+        return (status, component_ref_list)
+
+    def release(self, component_ref):
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        return status
+
+    def get_parameter(self, component_ref):
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        parameter_list = ["None"]
+        return (status, parameter_list)
+
+    def set_parameter(self, component_ref, parameters):
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        command_id = "0"
+        return (status, command_id)
+
+    def execute(self, command_unit_list):
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        return status
+
+    def get_command_result(self, command_id, condition):
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        results = ["None"]
+        return (status, results)
+
+
+class QueryIF(RoIS_HRI.QueryIF):
+    """
+    class QueryIF(object):
+    """
+
+    def __init__(self, Engine):
+        self._engine = Engine
+
+    def query(self, query_type, condition):
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        results = ["None"]
+        return (status, results)
+
+
+class EventIF(RoIS_HRI.EventIF):
+    """
+    class QueryIF(object):
+    """
+
+    def __init__(self, Engine):
+        self._engine = Engine
+
+    def subscribe(self, event_type, condition):
+        """
+        subscribe(self, event_type, condition) -> (status,subscribe_id)
+        """
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        subscribe_id = "0"
+        return (status, subscribe_id)
+
+    def unsubscribe(self, subscribe_id):
+        """
+        unsubscribe(self,subscribe_id) -> status
+        """
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        return status
+
+    def get_event_detail(self, event_id, condition):
+        """
+        get_event_detail(self,event_id,condition) -> (status,results)
+        """
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        results = ["None"]
+        return (status, results)
+
+
+class IF(SystemIF, CommandIF, QueryIF, EventIF):
+    """IF
+    """
+    pass
+
+
+class IF_server:
+    """IF_Server
+    """
+    def __init__(self, port):
+        self._addr = os.getenv("HRIENGINE")
+        self._server = ThreadingXMLRPCServer(
+            ("0.0.0.0", port), logRequests=False)
+
+    def run(self, _IF):
+        """IF_Server
+        """
+        self._server.register_instance(_IF)
+        self._server.register_introspection_functions()
+        # print("server running")
+        self._server.serve_forever()
+
+
+class RouteGuidanceEngine:
+    """IF_Server
+    """
+    def __init__(self):
+        self.state = False
+
+
+def test_engine(port):
+    """test_engine
+    """
+    IF_server(port).run(IF(RouteGuidanceEngine()))
+
+
+if __name__ == "__main__":
+    test_engine(8000)
-- 
GitLab


From 9a20437c14574df5d042aba3774bee2d6cf2309e Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Fri, 18 Oct 2019 16:17:44 +0900
Subject: [PATCH 02/20] Include custom client into IF class

---
 goal_sender/scripts/route_guidance_engine.py | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py
index 3da9e8e5..49eaab04 100644
--- a/goal_sender/scripts/route_guidance_engine.py
+++ b/goal_sender/scripts/route_guidance_engine.py
@@ -165,10 +165,16 @@ class EventIF(RoIS_HRI.EventIF):
         return (status, results)
 
 
+from goal_sender_client import GoalSenderClient as NavClient
+
 class IF(SystemIF, CommandIF, QueryIF, EventIF):
     """IF
     """
-    pass
+    def __init__(self, Engine):
+        super().__init__(Engine)
+        self.compoent_clients = {
+            'Navigation': NavClient('http://localhost:8001')
+        }
 
 
 class IF_server:
-- 
GitLab


From ca3c6c6083a44cb738dddf99fc7810ebda5eb58c Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Fri, 18 Oct 2019 17:01:39 +0900
Subject: [PATCH 03/20] Modify GoalSenderROS to response request of 'get_state'

---
 goal_sender/scripts/goal_sender_ros.py | 32 ++++++++++++++++++--------
 1 file changed, 23 insertions(+), 9 deletions(-)

diff --git a/goal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py
index 014bce3a..7958e1d9 100644
--- a/goal_sender/scripts/goal_sender_ros.py
+++ b/goal_sender/scripts/goal_sender_ros.py
@@ -2,11 +2,13 @@
 
 import rospy
 import actionlib
-from actionlib_msgs.msg import GoalStatus
 
 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
 from geometry_msgs.msg import PoseStamped
 
+from actionlib_msgs.msg import GoalStatus       as g_state
+from pyrois.RoIS_Common import Component_Status as c_state
+
 def pose_to_goal(pose):
     goal = MoveBaseGoal()
     goal.target_pose.header.frame_id = 'map'
@@ -22,20 +24,32 @@ def pose_to_goal(pose):
 
 
 class GoalSenderROS(object):
+    # Table for conversion from GoalStatus to Component_Status
+    # http://docs.ros.org/kinetic/api/actionlib_msgs/html/msg/GoalStatus.html
+    GoalSenderState = {
+        g_state.PENDING:    c_state.READY,
+        g_state.ACTIVE:     c_state.BUSY,
+        g_state.PREEMPTED:  c_state.READY,
+        g_state.SUCCEEDED:  c_state.READY,
+        g_state.ABORTED:    c_state.ERROR,
+        g_state.PREEMPTING: c_state.BUSY,
+        g_state.RECALLING:  c_state.BUSY,
+        g_state.RECALLED:   c_state.READY,
+        g_state.LOST:       c_state.ERROR
+    }
+
     def __init__(self):
         self.action_client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
-        #  self.action_client.wait_for_server()
+        self.action_client.wait_for_server()
 
     def send_goal(self):
         pose = [(10.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)]
         goal = pose_to_goal(pose)
-        state = self.action_client.send_goal_and_wait(goal)
-        if state == GoalStatus.SUCCEEDED:
-            rospy.loginfo("state: SUCCEEDED!!")
-        elif state == GoalStatus.ABORTED:
-            rospy.loginfo("state: ABORTED...")
-        else:
-            rospy.loginfo("state: {}".format(state))
+        self.action_client.send_goal(goal)
+
+    def get_goalsender_state(self):
+       state = self.action_client.get_state()
+       return GoalSenderROS.GoalSenderState[state]
 
 #  If you remove this comment out, it does not work.
 #  rospy.init_node('goal_sender')
-- 
GitLab


From f470e8e9ac446122432ca6470cb4b53d951d7a7d Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Tue, 22 Oct 2019 17:02:35 +0900
Subject: [PATCH 04/20] Add sample navigation component

---
 goal_sender/scripts/Navigation.py        | 141 +++++++++++++++++++++++
 goal_sender/scripts/Navigation_client.py | 107 +++++++++++++++++
 2 files changed, 248 insertions(+)
 create mode 100755 goal_sender/scripts/Navigation.py
 create mode 100644 goal_sender/scripts/Navigation_client.py

diff --git a/goal_sender/scripts/Navigation.py b/goal_sender/scripts/Navigation.py
new file mode 100755
index 00000000..ff81f2e8
--- /dev/null
+++ b/goal_sender/scripts/Navigation.py
@@ -0,0 +1,141 @@
+#!/usr/bin/env python
+
+# Navigation.py
+#
+# Copyright 2018 Eiichi Inohira
+# This software may be modified and distributed under the terms
+# of the MIT license
+#
+# For python3
+# For HRI Component
+
+"""Navigation
+"""
+
+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, target_position, time_limit, routing_policy):
+        self._component.Target_Position = target_position
+        self._component.Time_Limit = time_limit
+        self._component.Routing_Policy = routing_policy
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        return status
+
+
+class Query(RoIS_Common.Query):
+    """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)
+    
+    def get_parameter(self):
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        return (status, self._component.Target_Position, self._component.Time_Limit, self._component.Routing_Policy)
+    
+
+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
+
+
+class Navigation(Event, Command, Query):
+    """Navigation
+    """
+    def __init__(self, c):
+        super().__init__(c)
+        self._component = c
+        self._component.Target_Position = [""]
+        self._component.Time_Limit = 10
+        self._component.Routing_Policy = ""
+
+
+class component:
+    """component
+    """
+    def __init__(self):
+        self._state = False
+
+
+def event_dispatch(n):
+    """event_dispatch
+    """
+
+
+def example_n(port):
+    """example_n
+    """
+    c = component()
+    n = Navigation(c)
+
+    # 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__':
+    example_n(8013)
diff --git a/goal_sender/scripts/Navigation_client.py b/goal_sender/scripts/Navigation_client.py
new file mode 100644
index 00000000..b384eddc
--- /dev/null
+++ b/goal_sender/scripts/Navigation_client.py
@@ -0,0 +1,107 @@
+# Navigation_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
+
+"""Navigation_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):
+        s = self._proxy.set_parameter(target_position, time_limit, routing_policy)
+        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, 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 Navigation_Client(Command, Query, Event):
+    """Navigation_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()
\ No newline at end of file
-- 
GitLab


From feeda2ff9fc51db9d570f089004aadcf81b6c0a5 Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Tue, 22 Oct 2019 17:59:09 +0900
Subject: [PATCH 05/20] Include ros node into Navigation Component server

---
 goal_sender/scripts/Navigation.py            | 65 ++++++--------------
 goal_sender/scripts/route_guidance_engine.py |  2 +-
 2 files changed, 21 insertions(+), 46 deletions(-)

diff --git a/goal_sender/scripts/Navigation.py b/goal_sender/scripts/Navigation.py
index ff81f2e8..e3267d30 100755
--- a/goal_sender/scripts/Navigation.py
+++ b/goal_sender/scripts/Navigation.py
@@ -9,11 +9,8 @@
 # For python3
 # For HRI Component
 
-"""Navigation
-"""
-
 import sys
-import queue
+#  import queue
 import time
 from datetime import datetime
 import threading
@@ -21,24 +18,22 @@ 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):
-        """ThreadingXMLRPCServer
-        """
         pass
 
 
 class Command(RoIS_Common.Command):
-    """Command
-    """
     def __init__(self, c):
         self._component = c
 
@@ -58,17 +53,8 @@ class Command(RoIS_Common.Command):
         status = RoIS_HRI.ReturnCode_t.OK.value
         return status
 
-    def set_parameter(self, target_position, time_limit, routing_policy):
-        self._component.Target_Position = target_position
-        self._component.Time_Limit = time_limit
-        self._component.Routing_Policy = routing_policy
-        status = RoIS_HRI.ReturnCode_t.OK.value
-        return status
-
 
 class Query(RoIS_Common.Query):
-    """Query
-    """
     def __init__(self, c):
         self._component = c
 
@@ -76,54 +62,42 @@ class Query(RoIS_Common.Query):
         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.Target_Position, self._component.Time_Limit, self._component.Routing_Policy)
-    
+
 
 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 poll_event(self):
+        #  msg = self.event_queue.get()
+        #  return msg
 
 class Navigation(Event, Command, Query):
-    """Navigation
-    """
     def __init__(self, c):
-        super().__init__(c)
+        super(Navigation, self).__init__(c)
         self._component = c
         self._component.Target_Position = [""]
         self._component.Time_Limit = 10
         self._component.Routing_Policy = ""
 
 
-class component:
-    """component
-    """
-    def __init__(self):
-        self._state = False
+def event_dispatch(n):
+    #  n.person_detected(datetime.now().isoformat(), 1)
+    time.sleep(0.1)
+    #  n.person_detected(datetime.now().isoformat(), 1)
 
 
-def event_dispatch(n):
-    """event_dispatch
-    """
+class Component(object):
+    pass
 
+import rospy
 
 def example_n(port):
-    """example_n
-    """
-    c = component()
-    n = Navigation(c)
+    print("Starting node...")
+    component = Component()
+    n = Navigation(component)
+    print("Navigation constructed")
 
     # start the timer to dispatch events
     t = threading.Timer(0.1, event_dispatch, args=(n,))
@@ -138,4 +112,5 @@ def example_n(port):
 
 
 if __name__ == '__main__':
+    rospy.init_node('navigation_component')
     example_n(8013)
diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py
index 49eaab04..c5a92bb5 100644
--- a/goal_sender/scripts/route_guidance_engine.py
+++ b/goal_sender/scripts/route_guidance_engine.py
@@ -165,7 +165,7 @@ class EventIF(RoIS_HRI.EventIF):
         return (status, results)
 
 
-from goal_sender_client import GoalSenderClient as NavClient
+from Navigation_client import Navigation_Client as NavClient
 
 class IF(SystemIF, CommandIF, QueryIF, EventIF):
     """IF
-- 
GitLab


From 446d01f69524c2a9e3d5417ce6f68ea5bffe5d16 Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Wed, 23 Oct 2019 13:06:17 +0900
Subject: [PATCH 06/20] Add set_parameter method to Navigation Component

To send goal to move_base
---
 goal_sender/scripts/Navigation.py        | 10 +++++++++-
 goal_sender/scripts/Navigation_client.py |  2 +-
 goal_sender/scripts/test.py              |  9 +++++----
 3 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/goal_sender/scripts/Navigation.py b/goal_sender/scripts/Navigation.py
index e3267d30..976efeac 100755
--- a/goal_sender/scripts/Navigation.py
+++ b/goal_sender/scripts/Navigation.py
@@ -73,14 +73,22 @@ class Event(RoIS_Common.Event):
         #  msg = self.event_queue.get()
         #  return msg
 
+from goal_sender_ros import GoalSenderROS
+
 class Navigation(Event, Command, Query):
     def __init__(self, c):
         super(Navigation, self).__init__(c)
         self._component = c
+        self._goal_sender = GoalSenderROS()
         self._component.Target_Position = [""]
         self._component.Time_Limit = 10
         self._component.Routing_Policy = ""
 
+    def set_parameter(self, target_position, time_limit, routing_policy):
+        self._goal_sender.send_goal()
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        return status
+
 
 def event_dispatch(n):
     #  n.person_detected(datetime.now().isoformat(), 1)
@@ -113,4 +121,4 @@ def example_n(port):
 
 if __name__ == '__main__':
     rospy.init_node('navigation_component')
-    example_n(8013)
+    example_n(8001)
diff --git a/goal_sender/scripts/Navigation_client.py b/goal_sender/scripts/Navigation_client.py
index b384eddc..3b27ea8d 100644
--- a/goal_sender/scripts/Navigation_client.py
+++ b/goal_sender/scripts/Navigation_client.py
@@ -104,4 +104,4 @@ class Navigation_Client(Command, Query, Event):
         self._proxy = xmlrpc.client.ServerProxy(self._uri)
         self._e_proxy = xmlrpc.client.ServerProxy(self._uri)
         self.events = []
-        # self.start_th()
\ No newline at end of file
+        # self.start_th()
diff --git a/goal_sender/scripts/test.py b/goal_sender/scripts/test.py
index 71b90752..73fdab26 100644
--- a/goal_sender/scripts/test.py
+++ b/goal_sender/scripts/test.py
@@ -1,8 +1,9 @@
 import time
-from goal_sender_client import GoalSenderClient
+from Navigation_client import Navigation_Client
 
-uri = "http://localhost:8000"
-client = GoalSenderClient(uri)
+uri = "http://localhost:8001"
+client = Navigation_Client(uri)
 
-time.sleep(3)
 print(client.start())
+time.sleep(3)
+print(client.set_parameter("ahi", "chuni", "dwa"))
-- 
GitLab


From 415c7b574b395b8e736dc2d8618c5d2456f5853c Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Wed, 23 Oct 2019 13:09:42 +0900
Subject: [PATCH 07/20] Rename test script to test_component

---
 goal_sender/scripts/{test.py => test_component.py} | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 rename goal_sender/scripts/{test.py => test_component.py} (100%)

diff --git a/goal_sender/scripts/test.py b/goal_sender/scripts/test_component.py
similarity index 100%
rename from goal_sender/scripts/test.py
rename to goal_sender/scripts/test_component.py
-- 
GitLab


From c8067f7ab1f0170e7ee1c2a87a79ffbd1d44ee98 Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Wed, 23 Oct 2019 13:10:08 +0900
Subject: [PATCH 08/20] Add specific set_parameter method to HRI Engine(IF)

---
 goal_sender/scripts/route_guidance_engine.py | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py
index c5a92bb5..51231546 100644
--- a/goal_sender/scripts/route_guidance_engine.py
+++ b/goal_sender/scripts/route_guidance_engine.py
@@ -14,6 +14,7 @@ from __future__ import print_function
 
 import os
 import sys
+import threading
 
 from pyrois import RoIS_HRI
 
@@ -176,6 +177,14 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF):
             'Navigation': NavClient('http://localhost:8001')
         }
 
+    def set_parameter(self, component_ref, parameters):
+        status = None
+        if not component_ref in self.compoent_clients.keys():
+            status = RoIS_HRI.ReturnCode_t.ERROR.value
+        else:
+            status = RoIS_HRI.ReturnCode_t.OK.value
+        return (status, "2")
+
 
 class IF_server:
     """IF_Server
-- 
GitLab


From b2502651271b581a87fc1d71c8527fedb3fe469d Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Wed, 23 Oct 2019 13:11:29 +0900
Subject: [PATCH 09/20] Add sample HRI Engine client

---
 .../scripts/route_guidance_engine_client.py   | 136 ++++++++++++++++++
 1 file changed, 136 insertions(+)
 create mode 100644 goal_sender/scripts/route_guidance_engine_client.py

diff --git a/goal_sender/scripts/route_guidance_engine_client.py b/goal_sender/scripts/route_guidance_engine_client.py
new file mode 100644
index 00000000..a1b00726
--- /dev/null
+++ b/goal_sender/scripts/route_guidance_engine_client.py
@@ -0,0 +1,136 @@
+# HRI_Engine_client.py
+#
+# Copyright 2017 Eiichi Inohira
+# This software may be modified and distributed under the terms
+# of the MIT license
+#
+# For python 3
+# For Service Application
+
+"""HRI_Engine_client
+"""
+import time
+import xmlrpc.client
+# from pyrois import Service_Application_Base_example
+
+from pyrois import RoIS_HRI
+#  import Service_Application_IF_test
+
+class SystemIF(RoIS_HRI.SystemIF):
+    """SystemIF
+    """
+    def __init__(self, proxy_obj):
+        self._proxy = proxy_obj
+
+    def connect(self):
+        status = RoIS_HRI.ReturnCode_t(self._proxy.connect())
+        return status
+
+    def disconnect(self):
+        status = RoIS_HRI.ReturnCode_t(self._proxy.disconnect())
+        return status
+
+    def get_profile(self, condition):
+        (s, profile) = self._proxy.get_profile(condition)
+        status = RoIS_HRI.ReturnCode_t(s)
+        return (status, profile)
+
+    def get_error_detail(self, error_id, condition):
+        (s, results) = self._proxy.get_error_detail(error_id, condition)
+        status = RoIS_HRI.ReturnCode_t(s)
+        return (status, results)
+
+
+class CommandIF(RoIS_HRI.CommandIF):
+    """CommandIF
+    """
+    def __init__(self, proxy_obj):
+        self._proxy = proxy_obj
+
+    def search(self, condition):
+        (s, component_ref_list) = self._proxy.search(condition)
+        status = RoIS_HRI.ReturnCode_t(s)
+        return (status, component_ref_list)
+
+    def bind(self, component_ref):
+        status = RoIS_HRI.ReturnCode_t(self._proxy.bind(component_ref))
+        return status
+
+    def bind_any(self, condition):
+        (s, component_ref_list) = self._proxy.search(condition)
+        status = RoIS_HRI.ReturnCode_t(s)
+        return (status, component_ref_list)
+
+    def release(self, component_ref):
+        status = RoIS_HRI.ReturnCode_t(self._proxy.release(component_ref))
+        return status
+
+    def get_parameter(self, component_ref):
+        (s, parameter_list) = self._proxy.get_parameter(component_ref)
+        status = RoIS_HRI.ReturnCode_t(s)
+        return (status, parameter_list)
+
+    def set_parameter(self, component_ref, parameters):
+        (s, command_id) = self._proxy.set_parameter(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)
+        return status
+
+    def get_command_result(self, command_id, condition):
+        (s, results) = self._proxy.get_command_result(command_id, condition)
+        status = RoIS_HRI.ReturnCode_t(s)
+        return (status, results)
+
+
+class QueryIF(RoIS_HRI.QueryIF):
+    """QueryIF
+    """
+    def __init__(self, proxy_obj):
+        self._proxy = proxy_obj
+
+    def query(self, query_type, condition):
+        (s, results) = self._proxy.query(query_type, condition)
+        status = RoIS_HRI.ReturnCode_t(s)
+        return (status, results)
+
+
+class EventIF(RoIS_HRI.EventIF):
+    """EventIF
+    """
+    def __init__(self, proxy_obj):
+        self._proxy = proxy_obj
+
+    def subscribe(self, event_type, condition):
+        (s, subscribe_id) = self._proxy.subscribe(event_type, condition)
+        status = RoIS_HRI.ReturnCode_t(s)
+        return (status, subscribe_id)
+
+    def unsubscribe(self, subscribe_id):
+        s = self._proxy.unsubscribe(subscribe_id)
+        status = RoIS_HRI.ReturnCode_t(s)
+        return status
+
+    def get_event_detail(self, event_id, condition):
+        (s, results) = self._proxy.get_event_detail(event_id, condition)
+        status = RoIS_HRI.ReturnCode_t(s)
+        return (status, results)
+
+
+#  class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_IF_test.Service_Application_IF):
+class IF(SystemIF, CommandIF, QueryIF, EventIF):
+    """IF
+    """
+    def __init__(self, uri):
+        self._uri = uri
+        self._proxy = xmlrpc.client.ServerProxy(self._uri)
+        # self.a = Service_Application_Base_example.Service_Application_Base()
+        #  Service_Application_IF_test.Service_Application_IF.__init__(self,uri)
+
+    def analysis_c_status(self):
+        s = self._proxy.analysis_c_status()
+        status = RoIS_HRI.ReturnCode_t(s)
+        return status
-- 
GitLab


From cdc21acf427d660e9cd0bea6bb2342b70d1eb710 Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Wed, 23 Oct 2019 13:14:48 +0900
Subject: [PATCH 10/20] Add test for HRI engine

---
 goal_sender/scripts/test_engine.py | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)
 create mode 100644 goal_sender/scripts/test_engine.py

diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py
new file mode 100644
index 00000000..5b2c33cd
--- /dev/null
+++ b/goal_sender/scripts/test_engine.py
@@ -0,0 +1,18 @@
+from route_guidance_engine_client import IF
+import time
+
+s = 'http://127.0.0.1:8000'
+# a = SystemIF(s)
+# b = CommandIF(s)
+# c = QueryIF(s)            
+# d = EventIF(s)
+i = IF(s)
+i.connect()
+i.search("engine_profile_test")
+print(i.bind('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis'))
+#i.get_parameter('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis')
+print(i.set_parameter('Navigation',["hello","<speak></speak>",20,"english","robotA"]))
+#print(i.analysis_c_status())
+time.sleep(3)
+i.disconnect()
+print("finish")
-- 
GitLab


From cd6cb599438eea0a6c648e45b892455615c2762e Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Wed, 23 Oct 2019 13:20:54 +0900
Subject: [PATCH 11/20] Add component's set_parameter with HRIEngine

---
 goal_sender/scripts/route_guidance_engine.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py
index 51231546..1058bd44 100644
--- a/goal_sender/scripts/route_guidance_engine.py
+++ b/goal_sender/scripts/route_guidance_engine.py
@@ -179,10 +179,11 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF):
 
     def set_parameter(self, component_ref, parameters):
         status = None
-        if not component_ref in self.compoent_clients.keys():
+        if not component_ref in self.compoent_clients:
             status = RoIS_HRI.ReturnCode_t.ERROR.value
         else:
-            status = RoIS_HRI.ReturnCode_t.OK.value
+            target_component_client = self.compoent_clients[component_ref]
+            status = target_component_client.set_parameter("A", "B", "C")
         return (status, "2")
 
 
-- 
GitLab


From a55efeb8233cadffa75b813983cebac25b5aef55 Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Wed, 23 Oct 2019 13:42:14 +0900
Subject: [PATCH 12/20] Fix for calling set_parameter of Navigation component

---
 goal_sender/scripts/Navigation_client.py     | 4 ++--
 goal_sender/scripts/route_guidance_engine.py | 2 +-
 goal_sender/scripts/test_engine.py           | 2 +-
 3 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/goal_sender/scripts/Navigation_client.py b/goal_sender/scripts/Navigation_client.py
index 3b27ea8d..7f0e97f5 100644
--- a/goal_sender/scripts/Navigation_client.py
+++ b/goal_sender/scripts/Navigation_client.py
@@ -41,8 +41,8 @@ class Command(RoIS_Common.Command):
         return status
 
     def set_parameter(self, target_position, time_limit, routing_policy):
-        s = self._proxy.set_parameter(target_position, time_limit, routing_policy)
-        status = RoIS_HRI.ReturnCode_t(s)
+        status = self._proxy.set_parameter(target_position, time_limit, routing_policy)
+        #  status = RoIS_HRI.ReturnCode_t(status)
         return status
     
 
diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py
index 1058bd44..a8c00537 100644
--- a/goal_sender/scripts/route_guidance_engine.py
+++ b/goal_sender/scripts/route_guidance_engine.py
@@ -183,7 +183,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF):
             status = RoIS_HRI.ReturnCode_t.ERROR.value
         else:
             target_component_client = self.compoent_clients[component_ref]
-            status = target_component_client.set_parameter("A", "B", "C")
+            status = target_component_client.set_parameter(*parameters)
         return (status, "2")
 
 
diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py
index 5b2c33cd..cc48766b 100644
--- a/goal_sender/scripts/test_engine.py
+++ b/goal_sender/scripts/test_engine.py
@@ -11,7 +11,7 @@ i.connect()
 i.search("engine_profile_test")
 print(i.bind('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis'))
 #i.get_parameter('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis')
-print(i.set_parameter('Navigation',["hello","<speak></speak>",20,"english","robotA"]))
+print(i.set_parameter('Navigation',["hello","<speak></speak>", "a"]))
 #print(i.analysis_c_status())
 time.sleep(3)
 i.disconnect()
-- 
GitLab


From 7ab1e319ecbf9b9560f1db1a28ee90d263bf83c1 Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Wed, 23 Oct 2019 13:55:56 +0900
Subject: [PATCH 13/20] Upgrade for dynamic target position

---
 goal_sender/scripts/Navigation.py      | 2 +-
 goal_sender/scripts/goal_sender_ros.py | 5 +++--
 goal_sender/scripts/test_engine.py     | 2 +-
 3 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/goal_sender/scripts/Navigation.py b/goal_sender/scripts/Navigation.py
index 976efeac..b1d10420 100755
--- a/goal_sender/scripts/Navigation.py
+++ b/goal_sender/scripts/Navigation.py
@@ -85,7 +85,7 @@ class Navigation(Event, Command, Query):
         self._component.Routing_Policy = ""
 
     def set_parameter(self, target_position, time_limit, routing_policy):
-        self._goal_sender.send_goal()
+        self._goal_sender.send_goal(target_position)
         status = RoIS_HRI.ReturnCode_t.OK.value
         return status
 
diff --git a/goal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py
index 7958e1d9..c6058e48 100644
--- a/goal_sender/scripts/goal_sender_ros.py
+++ b/goal_sender/scripts/goal_sender_ros.py
@@ -42,8 +42,9 @@ class GoalSenderROS(object):
         self.action_client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
         self.action_client.wait_for_server()
 
-    def send_goal(self):
-        pose = [(10.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)]
+    def send_goal(self, pose_xy):
+        pose_xy.append(0)
+        pose = [pose_xy, (0.0, 0.0, 0.0, 1.0)]
         goal = pose_to_goal(pose)
         self.action_client.send_goal(goal)
 
diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py
index cc48766b..3ef202a8 100644
--- a/goal_sender/scripts/test_engine.py
+++ b/goal_sender/scripts/test_engine.py
@@ -11,7 +11,7 @@ i.connect()
 i.search("engine_profile_test")
 print(i.bind('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis'))
 #i.get_parameter('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis')
-print(i.set_parameter('Navigation',["hello","<speak></speak>", "a"]))
+print(i.set_parameter('Navigation',[[0, 5], "<speak></speak>", "a"]))
 #print(i.analysis_c_status())
 time.sleep(3)
 i.disconnect()
-- 
GitLab


From b320f7d1fa248fdda3c43b8b6f0e9d32cb1f54e3 Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Wed, 23 Oct 2019 14:08:47 +0900
Subject: [PATCH 14/20] Add test of multiple destination

---
 goal_sender/scripts/test_engine.py | 17 +++++++++++------
 1 file changed, 11 insertions(+), 6 deletions(-)

diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py
index 3ef202a8..7760dad5 100644
--- a/goal_sender/scripts/test_engine.py
+++ b/goal_sender/scripts/test_engine.py
@@ -1,18 +1,23 @@
+# 1. launch navigation.
+# 2. execute "rosrun (this_package) Navigation.py".
+# 3. run route_guidance_engine server.
+# 4. run this test.
+
+
 from route_guidance_engine_client import IF
 import time
 
 s = 'http://127.0.0.1:8000'
-# a = SystemIF(s)
-# b = CommandIF(s)
-# c = QueryIF(s)            
-# d = EventIF(s)
 i = IF(s)
 i.connect()
 i.search("engine_profile_test")
 print(i.bind('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis'))
 #i.get_parameter('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis')
-print(i.set_parameter('Navigation',[[0, 5], "<speak></speak>", "a"]))
+destinations = [[10, 0], [0, 20], [20, 20], [0, 0]]
+for dest in destinations:
+    print(i.set_parameter('Navigation',[dest, "", ""]))
+    time.sleep(3)
+
 #print(i.analysis_c_status())
-time.sleep(3)
 i.disconnect()
 print("finish")
-- 
GitLab


From 3841f7059abf18bca8aa49419b81361b91eac737 Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Wed, 23 Oct 2019 15:52:44 +0900
Subject: [PATCH 15/20] Check navigation status with "component_status"

---
 goal_sender/scripts/Navigation.py                   |  7 +++++++
 goal_sender/scripts/Navigation_client.py            |  9 +++++++--
 goal_sender/scripts/route_guidance_engine.py        |  7 +++++++
 goal_sender/scripts/route_guidance_engine_client.py | 12 +++++++++---
 goal_sender/scripts/test_engine.py                  |  7 +++++++
 5 files changed, 37 insertions(+), 5 deletions(-)

diff --git a/goal_sender/scripts/Navigation.py b/goal_sender/scripts/Navigation.py
index b1d10420..d40706b9 100755
--- a/goal_sender/scripts/Navigation.py
+++ b/goal_sender/scripts/Navigation.py
@@ -89,6 +89,13 @@ class Navigation(Event, Command, Query):
         status = RoIS_HRI.ReturnCode_t.OK.value
         return status
 
+    def component_status(self):
+        print("In Navigation server")
+        status = RoIS_HRI.ReturnCode_t.OK.value
+        c_status = self._goal_sender.get_goalsender_state().value
+        print("result: {}".format((status, c_status)))
+        return (status, c_status)
+
 
 def event_dispatch(n):
     #  n.person_detected(datetime.now().isoformat(), 1)
diff --git a/goal_sender/scripts/Navigation_client.py b/goal_sender/scripts/Navigation_client.py
index 7f0e97f5..322980b3 100644
--- a/goal_sender/scripts/Navigation_client.py
+++ b/goal_sender/scripts/Navigation_client.py
@@ -42,6 +42,7 @@ class Command(RoIS_Common.Command):
 
     def set_parameter(self, target_position, time_limit, routing_policy):
         status = self._proxy.set_parameter(target_position, time_limit, routing_policy)
+        print("Navigation_Client: ", status)
         #  status = RoIS_HRI.ReturnCode_t(status)
         return status
     
@@ -54,8 +55,12 @@ class Query(RoIS_Common.Query):
         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))
+        print("In navigation client")
+        print(dir(self._proxy))
+        status, c_status = self._proxy.component_status()
+        print("In navigation client(End)")
+        #  return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status))
+        return (status, c_status)
     
     def get_parameter(self):
         (status, target_position, time_limit, routing_policy) = self._proxy.get_parameter()
diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py
index a8c00537..0f28d63a 100644
--- a/goal_sender/scripts/route_guidance_engine.py
+++ b/goal_sender/scripts/route_guidance_engine.py
@@ -186,6 +186,13 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF):
             status = target_component_client.set_parameter(*parameters)
         return (status, "2")
 
+    ### Remove ###
+    def get_navi_status(self):
+        print("In client get_navi_status")
+        result = self.compoent_clients['Navigation'].component_status()
+        print("AHIAHI: {}".format(result))
+        return result
+    ##############
 
 class IF_server:
     """IF_Server
diff --git a/goal_sender/scripts/route_guidance_engine_client.py b/goal_sender/scripts/route_guidance_engine_client.py
index a1b00726..4127b7f4 100644
--- a/goal_sender/scripts/route_guidance_engine_client.py
+++ b/goal_sender/scripts/route_guidance_engine_client.py
@@ -13,7 +13,7 @@ import time
 import xmlrpc.client
 # from pyrois import Service_Application_Base_example
 
-from pyrois import RoIS_HRI
+from pyrois import RoIS_HRI, RoIS_Common
 #  import Service_Application_IF_test
 
 class SystemIF(RoIS_HRI.SystemIF):
@@ -71,8 +71,8 @@ class CommandIF(RoIS_HRI.CommandIF):
         return (status, parameter_list)
 
     def set_parameter(self, component_ref, parameters):
-        (s, command_id) = self._proxy.set_parameter(component_ref, parameters)
-        status = RoIS_HRI.ReturnCode_t(s)
+        (status, command_id) = self._proxy.set_parameter(component_ref, parameters)
+        #  status = RoIS_HRI.ReturnCode_t(s)
         return (status, command_id)
 
     def execute(self, command_unit_list):
@@ -134,3 +134,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF):
         s = self._proxy.analysis_c_status()
         status = RoIS_HRI.ReturnCode_t(s)
         return status
+
+    ### Remove ###
+    def get_navi_status(self):
+        status, c_status = self._proxy.get_navi_status()
+        return RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status)
+    ##############
diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py
index 7760dad5..3ae9e1a4 100644
--- a/goal_sender/scripts/test_engine.py
+++ b/goal_sender/scripts/test_engine.py
@@ -6,6 +6,7 @@
 
 from route_guidance_engine_client import IF
 import time
+from pyrois.RoIS_Common import Component_Status
 
 s = 'http://127.0.0.1:8000'
 i = IF(s)
@@ -16,6 +17,12 @@ print(i.bind('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis'))
 destinations = [[10, 0], [0, 20], [20, 20], [0, 0]]
 for dest in destinations:
     print(i.set_parameter('Navigation',[dest, "", ""]))
+    navi_status = Component_Status.BUSY
+    print(navi_status)
+    while navi_status != Component_Status.READY:
+        navi_status = i.get_navi_status()
+        print(navi_status)
+        time.sleep(1)
     time.sleep(3)
 
 #print(i.analysis_c_status())
-- 
GitLab


From ff226a69046a6e74d20955a6b9e3b77a9da92683 Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Wed, 23 Oct 2019 15:57:37 +0900
Subject: [PATCH 16/20] Fix status upgrade process intest

---
 goal_sender/scripts/test_engine.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py
index 3ae9e1a4..25d50637 100644
--- a/goal_sender/scripts/test_engine.py
+++ b/goal_sender/scripts/test_engine.py
@@ -18,10 +18,10 @@ destinations = [[10, 0], [0, 20], [20, 20], [0, 0]]
 for dest in destinations:
     print(i.set_parameter('Navigation',[dest, "", ""]))
     navi_status = Component_Status.BUSY
-    print(navi_status)
+    print("TEST: status = {}".format(navi_status))
     while navi_status != Component_Status.READY:
-        navi_status = i.get_navi_status()
-        print(navi_status)
+        server_status, navi_status = i.get_navi_status()
+        print("TEST: status = {}".format(navi_status))
         time.sleep(1)
     time.sleep(3)
 
-- 
GitLab


From 96fd916975179712f1a55f2f51c174cce54f5045 Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Wed, 23 Oct 2019 16:00:34 +0900
Subject: [PATCH 17/20] Remove code for debug

---
 goal_sender/scripts/Navigation.py            | 2 --
 goal_sender/scripts/Navigation_client.py     | 4 ----
 goal_sender/scripts/route_guidance_engine.py | 5 +----
 3 files changed, 1 insertion(+), 10 deletions(-)

diff --git a/goal_sender/scripts/Navigation.py b/goal_sender/scripts/Navigation.py
index d40706b9..879b2bdf 100755
--- a/goal_sender/scripts/Navigation.py
+++ b/goal_sender/scripts/Navigation.py
@@ -90,10 +90,8 @@ class Navigation(Event, Command, Query):
         return status
 
     def component_status(self):
-        print("In Navigation server")
         status = RoIS_HRI.ReturnCode_t.OK.value
         c_status = self._goal_sender.get_goalsender_state().value
-        print("result: {}".format((status, c_status)))
         return (status, c_status)
 
 
diff --git a/goal_sender/scripts/Navigation_client.py b/goal_sender/scripts/Navigation_client.py
index 322980b3..87063ee9 100644
--- a/goal_sender/scripts/Navigation_client.py
+++ b/goal_sender/scripts/Navigation_client.py
@@ -42,7 +42,6 @@ class Command(RoIS_Common.Command):
 
     def set_parameter(self, target_position, time_limit, routing_policy):
         status = self._proxy.set_parameter(target_position, time_limit, routing_policy)
-        print("Navigation_Client: ", status)
         #  status = RoIS_HRI.ReturnCode_t(status)
         return status
     
@@ -55,10 +54,7 @@ class Query(RoIS_Common.Query):
         self._proxy = xmlrpc.client.ServerProxy(self._uri)
 
     def component_status(self):
-        print("In navigation client")
-        print(dir(self._proxy))
         status, c_status = self._proxy.component_status()
-        print("In navigation client(End)")
         #  return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status))
         return (status, c_status)
     
diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py
index 0f28d63a..3629cd95 100644
--- a/goal_sender/scripts/route_guidance_engine.py
+++ b/goal_sender/scripts/route_guidance_engine.py
@@ -188,10 +188,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF):
 
     ### Remove ###
     def get_navi_status(self):
-        print("In client get_navi_status")
-        result = self.compoent_clients['Navigation'].component_status()
-        print("AHIAHI: {}".format(result))
-        return result
+        return self.compoent_clients['Navigation'].component_status()
     ##############
 
 class IF_server:
-- 
GitLab


From 34f06fbc4de4b89497cc9f59881390b715397c6e Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Wed, 23 Oct 2019 16:09:32 +0900
Subject: [PATCH 18/20] Fix status type for XMLRPC

---
 goal_sender/scripts/Navigation_client.py            | 5 ++---
 goal_sender/scripts/route_guidance_engine.py        | 5 +++--
 goal_sender/scripts/route_guidance_engine_client.py | 2 +-
 3 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/goal_sender/scripts/Navigation_client.py b/goal_sender/scripts/Navigation_client.py
index 87063ee9..fc616a48 100644
--- a/goal_sender/scripts/Navigation_client.py
+++ b/goal_sender/scripts/Navigation_client.py
@@ -42,7 +42,7 @@ class Command(RoIS_Common.Command):
 
     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)
+        status = RoIS_HRI.ReturnCode_t(status)
         return status
     
 
@@ -55,8 +55,7 @@ class Query(RoIS_Common.Query):
 
     def component_status(self):
         status, c_status = self._proxy.component_status()
-        #  return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status))
-        return (status, c_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()
diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py
index 3629cd95..e3237a24 100644
--- a/goal_sender/scripts/route_guidance_engine.py
+++ b/goal_sender/scripts/route_guidance_engine.py
@@ -183,12 +183,13 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF):
             status = RoIS_HRI.ReturnCode_t.ERROR.value
         else:
             target_component_client = self.compoent_clients[component_ref]
-            status = target_component_client.set_parameter(*parameters)
+            status = target_component_client.set_parameter(*parameters).value
         return (status, "2")
 
     ### Remove ###
     def get_navi_status(self):
-        return self.compoent_clients['Navigation'].component_status()
+        status, c_status = self.compoent_clients['Navigation'].component_status()
+        return (status.value, c_status.value)
     ##############
 
 class IF_server:
diff --git a/goal_sender/scripts/route_guidance_engine_client.py b/goal_sender/scripts/route_guidance_engine_client.py
index 4127b7f4..73786572 100644
--- a/goal_sender/scripts/route_guidance_engine_client.py
+++ b/goal_sender/scripts/route_guidance_engine_client.py
@@ -72,7 +72,7 @@ class CommandIF(RoIS_HRI.CommandIF):
 
     def set_parameter(self, component_ref, parameters):
         (status, command_id) = self._proxy.set_parameter(component_ref, parameters)
-        #  status = RoIS_HRI.ReturnCode_t(s)
+        status = RoIS_HRI.ReturnCode_t(status)
         return (status, command_id)
 
     def execute(self, command_unit_list):
-- 
GitLab


From 1760dae8cc0d5766909c96a9ee20dccaa6cf86ee Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Wed, 23 Oct 2019 16:36:52 +0900
Subject: [PATCH 19/20] Remake list of goal position

---
 goal_sender/scripts/test_engine.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py
index 25d50637..006b76c9 100644
--- a/goal_sender/scripts/test_engine.py
+++ b/goal_sender/scripts/test_engine.py
@@ -14,7 +14,7 @@ i.connect()
 i.search("engine_profile_test")
 print(i.bind('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis'))
 #i.get_parameter('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis')
-destinations = [[10, 0], [0, 20], [20, 20], [0, 0]]
+destinations = [[5, 0], [10, 0], [25, 0], [25, 10]]
 for dest in destinations:
     print(i.set_parameter('Navigation',[dest, "", ""]))
     navi_status = Component_Status.BUSY
-- 
GitLab


From 4f510e15e3cb2fa2ee8e8befadf43ab9ba65dfef Mon Sep 17 00:00:00 2001
From: tanacchi <q111026d@mail.kyutech.jp>
Date: Wed, 23 Oct 2019 16:44:53 +0900
Subject: [PATCH 20/20] Remove unnecessary files

---
 goal_sender/scripts/goal_sender_client.py | 104 -------------------
 goal_sender/scripts/goal_sender_node.py   | 116 ----------------------
 goal_sender/scripts/goal_sender_ros.py    |  56 -----------
 3 files changed, 276 deletions(-)
 delete mode 100644 goal_sender/scripts/goal_sender_client.py
 delete mode 100755 goal_sender/scripts/goal_sender_node.py
 delete mode 100644 goal_sender/scripts/goal_sender_ros.py

diff --git a/goal_sender/scripts/goal_sender_client.py b/goal_sender/scripts/goal_sender_client.py
deleted file mode 100644
index 239def84..00000000
--- a/goal_sender/scripts/goal_sender_client.py
+++ /dev/null
@@ -1,104 +0,0 @@
-# goal_sender_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
-
-"""goal_sender_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
-
-
-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))
-
-
-class Event(RoIS_Common.Event):
-    """Event
-    """
-    def __init__(self, uri):
-        self._uri = uri
-        self._e_proxy = xmlrpc.client.ServerProxy(self._uri)
-        # 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 == 'person_detected':
-            self.person_detected(params[0], params[1])
-
-    def person_detected(self, timestamp, number):
-        self.events.append((timestamp,number))
-        print("person_detected",timestamp,number)
-        # self.logger.debug('received completed event'
-        #                 + command_id
-        #                 + RoIS_Service.Completed_Status(status).name)
-
-
-class GoalSenderClient(Command, Query, Event):
-    """GoalSenderClient
-    """
-    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/goal_sender_node.py b/goal_sender/scripts/goal_sender_node.py
deleted file mode 100755
index 07c545cf..00000000
--- a/goal_sender/scripts/goal_sender_node.py
+++ /dev/null
@@ -1,116 +0,0 @@
-#!/usr/bin/env python
-
-# goal_sender_node.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):
-        print("Sending goal...")
-        self._component.send_goal()
-        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
-
-    #  def person_detected(self, timestamp, number):
-        #  msg = xmlrpc.client.dumps((timestamp, number), 'person_detected')
-        #  self.event_queue.put(msg)
-
-
-class GoalSender(Event, Command, Query):
-    pass
-
-
-def event_dispatch(gs):
-    #  gs.person_detected(datetime.now().isoformat(), 1)
-    time.sleep(0.1)
-    #  gs.person_detected(datetime.now().isoformat(), 1)
-
-
-import rospy
-from goal_sender_ros import GoalSenderROS
-
-def example_gs(port):
-    print("Starting node...")
-    component = GoalSenderROS()
-    gs = GoalSender(component)
-    print("Goalsender constructed")
-
-    # start the timer to dispatch events
-    t = threading.Timer(0.1, event_dispatch, args=(gs,))
-    t.start()
-
-    # start the XML-RPC server
-    server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False)
-    server.register_instance(gs)
-    server.register_introspection_functions()
-    server.register_multicall_functions()
-    server.serve_forever()
-
-
-if __name__ == '__main__':
-    rospy.init_node('goal_sender_node')
-    example_gs(8000)
diff --git a/goal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py
deleted file mode 100644
index c6058e48..00000000
--- a/goal_sender/scripts/goal_sender_ros.py
+++ /dev/null
@@ -1,56 +0,0 @@
-#/usr/bin/env python
-
-import rospy
-import actionlib
-
-from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
-from geometry_msgs.msg import PoseStamped
-
-from actionlib_msgs.msg import GoalStatus       as g_state
-from pyrois.RoIS_Common import Component_Status as c_state
-
-def pose_to_goal(pose):
-    goal = MoveBaseGoal()
-    goal.target_pose.header.frame_id = 'map'
-    goal.target_pose.pose.position.x = pose[0][0]
-    goal.target_pose.pose.position.y = pose[0][1]
-    goal.target_pose.pose.position.z = pose[0][2]
-    goal.target_pose.pose.orientation.x = pose[1][0]
-    goal.target_pose.pose.orientation.y = pose[1][1]
-    goal.target_pose.pose.orientation.z = pose[1][2]
-    goal.target_pose.pose.orientation.w = pose[1][3]
-
-    return goal
-
-
-class GoalSenderROS(object):
-    # Table for conversion from GoalStatus to Component_Status
-    # http://docs.ros.org/kinetic/api/actionlib_msgs/html/msg/GoalStatus.html
-    GoalSenderState = {
-        g_state.PENDING:    c_state.READY,
-        g_state.ACTIVE:     c_state.BUSY,
-        g_state.PREEMPTED:  c_state.READY,
-        g_state.SUCCEEDED:  c_state.READY,
-        g_state.ABORTED:    c_state.ERROR,
-        g_state.PREEMPTING: c_state.BUSY,
-        g_state.RECALLING:  c_state.BUSY,
-        g_state.RECALLED:   c_state.READY,
-        g_state.LOST:       c_state.ERROR
-    }
-
-    def __init__(self):
-        self.action_client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
-        self.action_client.wait_for_server()
-
-    def send_goal(self, pose_xy):
-        pose_xy.append(0)
-        pose = [pose_xy, (0.0, 0.0, 0.0, 1.0)]
-        goal = pose_to_goal(pose)
-        self.action_client.send_goal(goal)
-
-    def get_goalsender_state(self):
-       state = self.action_client.get_state()
-       return GoalSenderROS.GoalSenderState[state]
-
-#  If you remove this comment out, it does not work.
-#  rospy.init_node('goal_sender')
-- 
GitLab