From c2f93baa2aabef334d4ec0c76a2acaa95788b756 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 25 Oct 2019 00:23:52 +0900 Subject: [PATCH 01/72] Add toplevel HRI Engine --- .../scripts/toplevel_route_guidance_engine.py | 220 ++++++++++++++++++ 1 file changed, 220 insertions(+) create mode 100644 goal_sender/scripts/toplevel_route_guidance_engine.py diff --git a/goal_sender/scripts/toplevel_route_guidance_engine.py b/goal_sender/scripts/toplevel_route_guidance_engine.py new file mode 100644 index 00000000..0e177242 --- /dev/null +++ b/goal_sender/scripts/toplevel_route_guidance_engine.py @@ -0,0 +1,220 @@ +# HRI_Engine_example.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 + +"""HRI_Engine_example +""" + +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) + +from route_guidance_engine_client import IF as EngineClient + +class IF(SystemIF, CommandIF, QueryIF, EventIF): + """IF + """ + def __init__(self, Engine): + super().__init__(Engine) + self.command_id = 0 + self.engine_clients = { + 'robot1': EngineClient('http://localhost:8010') + } + for engine_client in self.engine_clients.values(): + engine_client.connect() + + def set_parameter(self, command_ref, parameters): + for engine_client in self.engine_clients: + engine_client.set(command_ref, *parameters) + status = RoIS_HRI.ReturnCode_t.OK.value + self.command_id += 1 + return (status, str(self.command_id)) + + +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 MyHRIE: + """IF_Server + """ + def __init__(self): + self.state = False + + +def test_engine(port): + """test_engine + """ + IF_server(port).run(IF(MyHRIE())) + + +if __name__ == "__main__": + pass -- GitLab From 622356204d2236c4d6626a4ff20d4c7a4d369a6b Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 25 Oct 2019 00:26:34 +0900 Subject: [PATCH 02/72] Change port number of lowlevel HRI Engine --- goal_sender/scripts/route_guidance_engine.py | 2 +- goal_sender/scripts/test_engine.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py index 335976b1..ab821d0a 100644 --- a/goal_sender/scripts/route_guidance_engine.py +++ b/goal_sender/scripts/route_guidance_engine.py @@ -225,4 +225,4 @@ def test_engine(port): if __name__ == "__main__": - test_engine(8000) + test_engine(8010) diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py index 006b76c9..a0e913f9 100644 --- a/goal_sender/scripts/test_engine.py +++ b/goal_sender/scripts/test_engine.py @@ -8,7 +8,7 @@ from route_guidance_engine_client import IF import time from pyrois.RoIS_Common import Component_Status -s = 'http://127.0.0.1:8000' +s = 'http://127.0.0.1:8010' i = IF(s) i.connect() i.search("engine_profile_test") -- GitLab From d7697164ab376ef1c6628db6fec14c3f87b5eb31 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 25 Oct 2019 00:30:07 +0900 Subject: [PATCH 03/72] Fix test for Navigation component --- goal_sender/scripts/test_component.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/goal_sender/scripts/test_component.py b/goal_sender/scripts/test_component.py index 73fdab26..25e4a77e 100644 --- a/goal_sender/scripts/test_component.py +++ b/goal_sender/scripts/test_component.py @@ -1,3 +1,7 @@ +# 1. launch navigation. +# 2. execute "rosrun (this_package) Navigation.py" +# 3. run this test + import time from Navigation_client import Navigation_Client @@ -6,4 +10,4 @@ client = Navigation_Client(uri) print(client.start()) time.sleep(3) -print(client.set_parameter("ahi", "chuni", "dwa")) +print(client.set_parameter([10, 0], "", "")) -- GitLab From 9012e09aefc35c36a1f460c3e44e6e981c87df41 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 25 Oct 2019 01:25:04 +0900 Subject: [PATCH 04/72] Fix passing arguments to lowlevel Engine client --- goal_sender/scripts/toplevel_route_guidance_engine.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/goal_sender/scripts/toplevel_route_guidance_engine.py b/goal_sender/scripts/toplevel_route_guidance_engine.py index 0e177242..db0043b3 100644 --- a/goal_sender/scripts/toplevel_route_guidance_engine.py +++ b/goal_sender/scripts/toplevel_route_guidance_engine.py @@ -179,8 +179,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): engine_client.connect() def set_parameter(self, command_ref, parameters): - for engine_client in self.engine_clients: - engine_client.set(command_ref, *parameters) + for engine_client in self.engine_clients.values(): + engine_client.set_parameter(command_ref, parameters) status = RoIS_HRI.ReturnCode_t.OK.value self.command_id += 1 return (status, str(self.command_id)) @@ -217,4 +217,4 @@ def test_engine(port): if __name__ == "__main__": - pass + test_engine(8000) -- GitLab From 27c9cdee9fdd305e342220806fa037399d826c46 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 25 Oct 2019 01:26:09 +0900 Subject: [PATCH 05/72] Add client of toplevel Engine client --- .../toplevel_route_guidance_engine_client.py | 128 ++++++++++++++++++ 1 file changed, 128 insertions(+) create mode 100644 goal_sender/scripts/toplevel_route_guidance_engine_client.py diff --git a/goal_sender/scripts/toplevel_route_guidance_engine_client.py b/goal_sender/scripts/toplevel_route_guidance_engine_client.py new file mode 100644 index 00000000..b522efb7 --- /dev/null +++ b/goal_sender/scripts/toplevel_route_guidance_engine_client.py @@ -0,0 +1,128 @@ +# 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 xmlrpc.client + +from pyrois import RoIS_HRI + + +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) + return (status, results) + + +class IF(SystemIF, CommandIF, QueryIF, EventIF): + """IF + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + +if __name__ == "__main__": + pass -- GitLab From f96994b2642a83c6eb1803d1c2390acfd8ddf779 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 25 Oct 2019 01:59:49 +0900 Subject: [PATCH 06/72] Add test for toplevel HRI Engine --- goal_sender/scripts/test_toplevel_engine.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) create mode 100644 goal_sender/scripts/test_toplevel_engine.py diff --git a/goal_sender/scripts/test_toplevel_engine.py b/goal_sender/scripts/test_toplevel_engine.py new file mode 100644 index 00000000..e57fb1ba --- /dev/null +++ b/goal_sender/scripts/test_toplevel_engine.py @@ -0,0 +1,20 @@ +# 1. launch navigation. +# 2. execute "rosrun (this_package) Navigation.py". +# 3. run route_guidance_engine server. +# 4. run toplevel_route_guidance_engine server. +# 5. run this test. + + +from toplevel_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) +i.connect() +time.sleep(3) +status = i.set_parameter('Navigation',[[25, 20], "", ""]) +print(status) + +i.disconnect() +print("finish") -- GitLab From 31f7e1d193db7985387a4a6449b491b7d563f430 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 25 Oct 2019 15:11:40 +0900 Subject: [PATCH 07/72] Use namespace group for controller-spawning --- rois_gazebo/config/controller.yaml | 89 +++++++++++++------------- rois_gazebo/launch/setup_gazebo.launch | 33 +++++----- 2 files changed, 59 insertions(+), 63 deletions(-) diff --git a/rois_gazebo/config/controller.yaml b/rois_gazebo/config/controller.yaml index d9ae1756..0c663c23 100644 --- a/rois_gazebo/config/controller.yaml +++ b/rois_gazebo/config/controller.yaml @@ -1,46 +1,45 @@ -roisbot: - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 - diff_drive_controller: - type : "diff_drive_controller/DiffDriveController" - left_wheel : 'left_wheel_joint' - right_wheel : 'right_wheel_joint' - pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] - twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] - - # Wheel separation and diameter. These are both optional. - # diff_drive_controller will attempt to read either one or both from the - # URDF if not specified as a parameter - wheel_separation : 0.3 - wheel_radius : 0.1 - - # Wheel separation and radius multipliers - wheel_separation_multiplier: 1.0 # default: 1.0 - wheel_radius_multiplier : 1.0 # default: 1.0 - - # Velocity commands timeout [s], default 0.5 - cmd_vel_timeout: 1.0 - - # Base frame_id - base_frame_id: base_footprint #default: base_link - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits : true - max_velocity : 2.0 # m/s - min_velocity : -2.0 # m/s - has_acceleration_limits: true - max_acceleration : 2.0 # m/s^2 - min_acceleration : -2.0 # m/s^2 - angular: - z: - has_velocity_limits : true - max_velocity : 1.0 # rad/s - min_velocity : -1.0 - has_acceleration_limits: true - max_acceleration : 1.0 # rad/s^2 - min_acceleration : -1.0 +diff_drive_controller: + type : "diff_drive_controller/DiffDriveController" + left_wheel : 'left_wheel_joint' + right_wheel : 'right_wheel_joint' + pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + + # Wheel separation and diameter. These are both optional. + # diff_drive_controller will attempt to read either one or both from the + # URDF if not specified as a parameter + wheel_separation : 0.3 + wheel_radius : 0.1 + + # Wheel separation and radius multipliers + wheel_separation_multiplier: 1.0 # default: 1.0 + wheel_radius_multiplier : 1.0 # default: 1.0 + + # Velocity commands timeout [s], default 0.5 + cmd_vel_timeout: 1.0 + + # Base frame_id + base_frame_id: base_footprint #default: base_link + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits : true + max_velocity : 2.0 # m/s + min_velocity : -2.0 # m/s + has_acceleration_limits: true + max_acceleration : 2.0 # m/s^2 + min_acceleration : -2.0 # m/s^2 + angular: + z: + has_velocity_limits : true + max_velocity : 1.0 # rad/s + min_velocity : -1.0 + has_acceleration_limits: true + max_acceleration : 1.0 # rad/s^2 + min_acceleration : -1.0 diff --git a/rois_gazebo/launch/setup_gazebo.launch b/rois_gazebo/launch/setup_gazebo.launch index 8b2b02db..e6b848ae 100644 --- a/rois_gazebo/launch/setup_gazebo.launch +++ b/rois_gazebo/launch/setup_gazebo.launch @@ -15,33 +15,30 @@ value="$(arg world_name)" /> - - - - - - + + + + + Date: Fri, 25 Oct 2019 15:55:38 +0900 Subject: [PATCH 08/72] Split model_spawner by namespace --- rois_gazebo/launch/setup_gazebo.launch | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/rois_gazebo/launch/setup_gazebo.launch b/rois_gazebo/launch/setup_gazebo.launch index e6b848ae..03bbca65 100644 --- a/rois_gazebo/launch/setup_gazebo.launch +++ b/rois_gazebo/launch/setup_gazebo.launch @@ -18,26 +18,29 @@ - - - + + output="screen"> + + + + + Date: Fri, 25 Oct 2019 16:22:17 +0900 Subject: [PATCH 09/72] Split launch file to spawn robot --- rois_gazebo/launch/setup_gazebo.launch | 35 ++++++-------------------- rois_gazebo/launch/spawn_robot.launch | 35 ++++++++++++++++++++++++++ 2 files changed, 43 insertions(+), 27 deletions(-) create mode 100644 rois_gazebo/launch/spawn_robot.launch diff --git a/rois_gazebo/launch/setup_gazebo.launch b/rois_gazebo/launch/setup_gazebo.launch index 03bbca65..3a675ed6 100644 --- a/rois_gazebo/launch/setup_gazebo.launch +++ b/rois_gazebo/launch/setup_gazebo.launch @@ -15,33 +15,14 @@ value="$(arg world_name)" /> - - - - - - - - - - - + + + + + + + + + + + + + + + + + + -- GitLab From e919c8b6bf41ddfc53588497f3190f8908143457 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 25 Oct 2019 16:34:45 +0900 Subject: [PATCH 10/72] Add argument to teleop launcher --- rois_gazebo/launch/teleop_keyboard.launch | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rois_gazebo/launch/teleop_keyboard.launch b/rois_gazebo/launch/teleop_keyboard.launch index ad051e88..467546c3 100644 --- a/rois_gazebo/launch/teleop_keyboard.launch +++ b/rois_gazebo/launch/teleop_keyboard.launch @@ -1,5 +1,6 @@ + + to="/$(arg robot_name)/diff_drive_controller/cmd_vel" /> -- GitLab From aa0deb076c0b5bbbc6707bc9cbce5d6a5bc95ad6 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 10:38:42 +0900 Subject: [PATCH 11/72] Add robot_name param to setup_gazebo launcher --- rois_gazebo/launch/setup_gazebo.launch | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rois_gazebo/launch/setup_gazebo.launch b/rois_gazebo/launch/setup_gazebo.launch index 3a675ed6..adbb6567 100644 --- a/rois_gazebo/launch/setup_gazebo.launch +++ b/rois_gazebo/launch/setup_gazebo.launch @@ -8,6 +8,9 @@ + + value="$(arg robot_name)" /> -- GitLab From c872c599eb2d85e86085b1dccecbb9d831ad7fbc Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 10:39:09 +0900 Subject: [PATCH 12/72] Remove unnecessary white space --- rois_gazebo/launch/spawn_robot.launch | 41 ++++++++++++++------------- 1 file changed, 22 insertions(+), 19 deletions(-) diff --git a/rois_gazebo/launch/spawn_robot.launch b/rois_gazebo/launch/spawn_robot.launch index 7b2b4054..7ae76564 100644 --- a/rois_gazebo/launch/spawn_robot.launch +++ b/rois_gazebo/launch/spawn_robot.launch @@ -1,35 +1,38 @@ - - - + + - - - -- GitLab From 552a6beabe7efd3a3e363ad8a9f8ec31f26bd71c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 10:59:23 +0900 Subject: [PATCH 13/72] Fix usage of property on xacro file --- rois_description/robot/roisbot.urdf.xacro | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/rois_description/robot/roisbot.urdf.xacro b/rois_description/robot/roisbot.urdf.xacro index b201f413..01f4b7a3 100644 --- a/rois_description/robot/roisbot.urdf.xacro +++ b/rois_description/robot/roisbot.urdf.xacro @@ -1,12 +1,12 @@ - - - - - - - + + + + + + + -- GitLab From fdf6e6820864a3754c7d2d611e04d6e3d9cce6f8 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 11:18:33 +0900 Subject: [PATCH 14/72] Chane default value of robot_name --- rois_gazebo/launch/setup_gazebo.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rois_gazebo/launch/setup_gazebo.launch b/rois_gazebo/launch/setup_gazebo.launch index adbb6567..613bedee 100644 --- a/rois_gazebo/launch/setup_gazebo.launch +++ b/rois_gazebo/launch/setup_gazebo.launch @@ -10,7 +10,7 @@ default="$(find rois_description)/world/c_corridor.sdf" /> + default="roisbot" /> Date: Tue, 29 Oct 2019 11:46:35 +0900 Subject: [PATCH 15/72] Include robot_description param into robot-namespace --- rois_gazebo/launch/spawn_robot.launch | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rois_gazebo/launch/spawn_robot.launch b/rois_gazebo/launch/spawn_robot.launch index 7ae76564..e4ce8e97 100644 --- a/rois_gazebo/launch/spawn_robot.launch +++ b/rois_gazebo/launch/spawn_robot.launch @@ -5,11 +5,11 @@ - + @@ -33,6 +33,6 @@ pkg="gazebo_ros" type="spawn_model" name="spawn_urdf" - args="-urdf -model $(arg robot_name) -param /robot_description" /> + args="-urdf -model $(arg robot_name) -param robot_description" /> -- GitLab From a5074fcbeda233710151c1d9cc039e79ae42616e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 12:14:25 +0900 Subject: [PATCH 16/72] Add specific urdf file for multiple robot --- rois_description/robot/robot1.urdf.xacro | 234 +++++++++++++++++++++++ 1 file changed, 234 insertions(+) create mode 100644 rois_description/robot/robot1.urdf.xacro diff --git a/rois_description/robot/robot1.urdf.xacro b/rois_description/robot/robot1.urdf.xacro new file mode 100644 index 00000000..29c6347e --- /dev/null +++ b/rois_description/robot/robot1.urdf.xacro @@ -0,0 +1,234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + 15 + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + 15 + + + + + + + + + + + + + + + + + + + + + + Gazebo/Blue + false + + 0.2 0 0 0 0 0 + true + 40 + + + + 500 + 1 + -${M_PI*3/4} + ${M_PI*3/4} + + + + 0.20 + 131 + 0.01 + + + + /scan + lrf_link + 0.9 + 130 + + + + + + + + robot1 + + + + + true + Gazebo/White + + + + + + true + Gazebo/Green + + + + + + true + Gazebo/Red + + + + + + true + Gazebo/Black + + + + + -- GitLab From ee19d28e2c4fe949ad51f0dbb3ee0659cf859350 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 12:17:11 +0900 Subject: [PATCH 17/72] Modify to read specific urdf file --- rois_gazebo/launch/setup_gazebo.launch | 8 ++++---- rois_gazebo/launch/spawn_robot.launch | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rois_gazebo/launch/setup_gazebo.launch b/rois_gazebo/launch/setup_gazebo.launch index 613bedee..29660266 100644 --- a/rois_gazebo/launch/setup_gazebo.launch +++ b/rois_gazebo/launch/setup_gazebo.launch @@ -1,16 +1,16 @@ + + default="$(find rois_description)/robot/$(arg robot_name).urdf.xacro" /> - + default="$(find rois_description)/robot/$(arg robot_name).urdf.xacro" /> Date: Tue, 29 Oct 2019 12:25:48 +0900 Subject: [PATCH 18/72] Add arguments of initial position --- rois_gazebo/launch/setup_gazebo.launch | 6 ++++++ rois_gazebo/launch/spawn_robot.launch | 8 +++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/rois_gazebo/launch/setup_gazebo.launch b/rois_gazebo/launch/setup_gazebo.launch index 29660266..e47afbfb 100644 --- a/rois_gazebo/launch/setup_gazebo.launch +++ b/rois_gazebo/launch/setup_gazebo.launch @@ -25,6 +25,12 @@ + + + + + + + args="-urdf -model $(arg robot_name) -param robot_description $(arg init_pos)" /> -- GitLab From b8918f2772ec75e9eade93e535a3a1a489f63245 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 13:34:37 +0900 Subject: [PATCH 19/72] Remove model config from setup_gazebo.launch --- rois_gazebo/launch/setup_gazebo.launch | 6 ------ 1 file changed, 6 deletions(-) diff --git a/rois_gazebo/launch/setup_gazebo.launch b/rois_gazebo/launch/setup_gazebo.launch index e47afbfb..b0444e44 100644 --- a/rois_gazebo/launch/setup_gazebo.launch +++ b/rois_gazebo/launch/setup_gazebo.launch @@ -2,9 +2,6 @@ - @@ -22,9 +19,6 @@ - -- GitLab From 7b6de589897cc7fefa76c35a9f4d1fa94984bc66 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 13:43:55 +0900 Subject: [PATCH 20/72] Remove "robot_name" argument from setup_gazebo --- rois_gazebo/launch/setup_gazebo.launch | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/rois_gazebo/launch/setup_gazebo.launch b/rois_gazebo/launch/setup_gazebo.launch index b0444e44..57e4b28a 100644 --- a/rois_gazebo/launch/setup_gazebo.launch +++ b/rois_gazebo/launch/setup_gazebo.launch @@ -1,7 +1,4 @@ - @@ -18,13 +15,13 @@ + value="robot1"/> + value="0" /> + value="0" /> Date: Tue, 29 Oct 2019 13:46:53 +0900 Subject: [PATCH 21/72] Add urdf file for robot2 and robot3 --- rois_description/robot/robot2.urdf.xacro | 234 +++++++++++++++++++++++ rois_description/robot/robot3.urdf.xacro | 234 +++++++++++++++++++++++ 2 files changed, 468 insertions(+) create mode 100644 rois_description/robot/robot2.urdf.xacro create mode 100644 rois_description/robot/robot3.urdf.xacro diff --git a/rois_description/robot/robot2.urdf.xacro b/rois_description/robot/robot2.urdf.xacro new file mode 100644 index 00000000..8f9f7474 --- /dev/null +++ b/rois_description/robot/robot2.urdf.xacro @@ -0,0 +1,234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + 15 + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + 15 + + + + + + + + + + + + + + + + + + + + + + Gazebo/Blue + false + + 0.2 0 0 0 0 0 + true + 40 + + + + 500 + 1 + -${M_PI*3/4} + ${M_PI*3/4} + + + + 0.20 + 131 + 0.01 + + + + /scan + lrf_link + 0.9 + 130 + + + + + + + + robot2 + + + + + true + Gazebo/White + + + + + + true + Gazebo/Green + + + + + + true + Gazebo/Red + + + + + + true + Gazebo/Black + + + + + diff --git a/rois_description/robot/robot3.urdf.xacro b/rois_description/robot/robot3.urdf.xacro new file mode 100644 index 00000000..5135eb3c --- /dev/null +++ b/rois_description/robot/robot3.urdf.xacro @@ -0,0 +1,234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + 15 + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + 15 + + + + + + + + + + + + + + + + + + + + + + Gazebo/Blue + false + + 0.2 0 0 0 0 0 + true + 40 + + + + 500 + 1 + -${M_PI*3/4} + ${M_PI*3/4} + + + + 0.20 + 131 + 0.01 + + + + /scan + lrf_link + 0.9 + 130 + + + + + + + + robot3 + + + + + true + Gazebo/White + + + + + + true + Gazebo/Green + + + + + + true + Gazebo/Red + + + + + + true + Gazebo/Black + + + + + -- GitLab From 42bd7e0bc539e8971f69782c6ead7f63e1c86f00 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 13:53:55 +0900 Subject: [PATCH 22/72] Add multi_robot mode to setup-gazebo launch --- rois_gazebo/launch/setup_gazebo.launch | 29 ++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/rois_gazebo/launch/setup_gazebo.launch b/rois_gazebo/launch/setup_gazebo.launch index 57e4b28a..95e9dfdb 100644 --- a/rois_gazebo/launch/setup_gazebo.launch +++ b/rois_gazebo/launch/setup_gazebo.launch @@ -5,6 +5,9 @@ + + + + + + + + + + + + + + + Date: Tue, 29 Oct 2019 14:17:21 +0900 Subject: [PATCH 23/72] Fix LaserScan topic name (Global => nonGlobal) --- rois_description/robot/robot1.urdf.xacro | 2 +- rois_description/robot/robot2.urdf.xacro | 2 +- rois_description/robot/robot3.urdf.xacro | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rois_description/robot/robot1.urdf.xacro b/rois_description/robot/robot1.urdf.xacro index 29c6347e..bf0d8b5c 100644 --- a/rois_description/robot/robot1.urdf.xacro +++ b/rois_description/robot/robot1.urdf.xacro @@ -188,7 +188,7 @@ - /scan + scan lrf_link 0.9 130 diff --git a/rois_description/robot/robot2.urdf.xacro b/rois_description/robot/robot2.urdf.xacro index 8f9f7474..52d45ef8 100644 --- a/rois_description/robot/robot2.urdf.xacro +++ b/rois_description/robot/robot2.urdf.xacro @@ -188,7 +188,7 @@ - /scan + scan lrf_link 0.9 130 diff --git a/rois_description/robot/robot3.urdf.xacro b/rois_description/robot/robot3.urdf.xacro index 5135eb3c..228362dd 100644 --- a/rois_description/robot/robot3.urdf.xacro +++ b/rois_description/robot/robot3.urdf.xacro @@ -188,7 +188,7 @@ - /scan + scan lrf_link 0.9 130 -- GitLab From a8d9bd1fb716aafdbfbe5370bbd41f744ddb6691 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 14:33:10 +0900 Subject: [PATCH 24/72] [WIP] Add use_multi_robot mode to navigation launcher --- rois_bringup/launch/navigation.launch | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/rois_bringup/launch/navigation.launch b/rois_bringup/launch/navigation.launch index 75644b21..9eda90da 100644 --- a/rois_bringup/launch/navigation.launch +++ b/rois_bringup/launch/navigation.launch @@ -2,6 +2,9 @@ + - + + - + + + -- GitLab From 6984739e3b26a028997e5a2b32856f6ede86de85 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 14:50:15 +0900 Subject: [PATCH 25/72] [TMP] Include map data into robot namespace --- rois_2dnav_gazebo/launch/amcl.launch | 1 + rois_bringup/launch/navigation.launch | 12 ++++++------ 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/rois_2dnav_gazebo/launch/amcl.launch b/rois_2dnav_gazebo/launch/amcl.launch index 3d9fb4fa..7a5e8039 100644 --- a/rois_2dnav_gazebo/launch/amcl.launch +++ b/rois_2dnav_gazebo/launch/amcl.launch @@ -1,5 +1,6 @@ + diff --git a/rois_bringup/launch/navigation.launch b/rois_bringup/launch/navigation.launch index 9eda90da..832b6898 100644 --- a/rois_bringup/launch/navigation.launch +++ b/rois_bringup/launch/navigation.launch @@ -6,12 +6,6 @@ name="use_multi_robot" default="true" /> - - + + -- GitLab From 8d7628dca206011b597fceb2b118a69a79c43a5f Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 14:57:59 +0900 Subject: [PATCH 26/72] Split controller file by robot name --- rois_gazebo/config/robot1_controller.yaml | 45 +++++++++++++++++++++++ rois_gazebo/config/robot2_controller.yaml | 45 +++++++++++++++++++++++ rois_gazebo/config/robot3_controller.yaml | 45 +++++++++++++++++++++++ rois_gazebo/launch/spawn_robot.launch | 2 +- 4 files changed, 136 insertions(+), 1 deletion(-) create mode 100644 rois_gazebo/config/robot1_controller.yaml create mode 100644 rois_gazebo/config/robot2_controller.yaml create mode 100644 rois_gazebo/config/robot3_controller.yaml diff --git a/rois_gazebo/config/robot1_controller.yaml b/rois_gazebo/config/robot1_controller.yaml new file mode 100644 index 00000000..1b608018 --- /dev/null +++ b/rois_gazebo/config/robot1_controller.yaml @@ -0,0 +1,45 @@ +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + +diff_drive_controller: + type : "diff_drive_controller/DiffDriveController" + left_wheel : 'left_wheel_joint' + right_wheel : 'right_wheel_joint' + pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + + # Wheel separation and diameter. These are both optional. + # diff_drive_controller will attempt to read either one or both from the + # URDF if not specified as a parameter + wheel_separation : 0.3 + wheel_radius : 0.1 + + # Wheel separation and radius multipliers + wheel_separation_multiplier: 1.0 # default: 1.0 + wheel_radius_multiplier : 1.0 # default: 1.0 + + # Velocity commands timeout [s], default 0.5 + cmd_vel_timeout: 1.0 + + # Base frame_id + base_frame_id: robot1/base_footprint #default: base_link + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits : true + max_velocity : 2.0 # m/s + min_velocity : -2.0 # m/s + has_acceleration_limits: true + max_acceleration : 2.0 # m/s^2 + min_acceleration : -2.0 # m/s^2 + angular: + z: + has_velocity_limits : true + max_velocity : 1.0 # rad/s + min_velocity : -1.0 + has_acceleration_limits: true + max_acceleration : 1.0 # rad/s^2 + min_acceleration : -1.0 diff --git a/rois_gazebo/config/robot2_controller.yaml b/rois_gazebo/config/robot2_controller.yaml new file mode 100644 index 00000000..b07792e4 --- /dev/null +++ b/rois_gazebo/config/robot2_controller.yaml @@ -0,0 +1,45 @@ +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + +diff_drive_controller: + type : "diff_drive_controller/DiffDriveController" + left_wheel : 'left_wheel_joint' + right_wheel : 'right_wheel_joint' + pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + + # Wheel separation and diameter. These are both optional. + # diff_drive_controller will attempt to read either one or both from the + # URDF if not specified as a parameter + wheel_separation : 0.3 + wheel_radius : 0.1 + + # Wheel separation and radius multipliers + wheel_separation_multiplier: 1.0 # default: 1.0 + wheel_radius_multiplier : 1.0 # default: 1.0 + + # Velocity commands timeout [s], default 0.5 + cmd_vel_timeout: 1.0 + + # Base frame_id + base_frame_id: robot2/base_footprint #default: base_link + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits : true + max_velocity : 2.0 # m/s + min_velocity : -2.0 # m/s + has_acceleration_limits: true + max_acceleration : 2.0 # m/s^2 + min_acceleration : -2.0 # m/s^2 + angular: + z: + has_velocity_limits : true + max_velocity : 1.0 # rad/s + min_velocity : -1.0 + has_acceleration_limits: true + max_acceleration : 1.0 # rad/s^2 + min_acceleration : -1.0 diff --git a/rois_gazebo/config/robot3_controller.yaml b/rois_gazebo/config/robot3_controller.yaml new file mode 100644 index 00000000..ac9d2dbf --- /dev/null +++ b/rois_gazebo/config/robot3_controller.yaml @@ -0,0 +1,45 @@ +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + +diff_drive_controller: + type : "diff_drive_controller/DiffDriveController" + left_wheel : 'left_wheel_joint' + right_wheel : 'right_wheel_joint' + pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + + # Wheel separation and diameter. These are both optional. + # diff_drive_controller will attempt to read either one or both from the + # URDF if not specified as a parameter + wheel_separation : 0.3 + wheel_radius : 0.1 + + # Wheel separation and radius multipliers + wheel_separation_multiplier: 1.0 # default: 1.0 + wheel_radius_multiplier : 1.0 # default: 1.0 + + # Velocity commands timeout [s], default 0.5 + cmd_vel_timeout: 1.0 + + # Base frame_id + base_frame_id: robot3/base_footprint #default: base_link + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits : true + max_velocity : 2.0 # m/s + min_velocity : -2.0 # m/s + has_acceleration_limits: true + max_acceleration : 2.0 # m/s^2 + min_acceleration : -2.0 # m/s^2 + angular: + z: + has_velocity_limits : true + max_velocity : 1.0 # rad/s + min_velocity : -1.0 + has_acceleration_limits: true + max_acceleration : 1.0 # rad/s^2 + min_acceleration : -1.0 diff --git a/rois_gazebo/launch/spawn_robot.launch b/rois_gazebo/launch/spawn_robot.launch index a453a091..9846c664 100644 --- a/rois_gazebo/launch/spawn_robot.launch +++ b/rois_gazebo/launch/spawn_robot.launch @@ -17,7 +17,7 @@ name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" /> - + Date: Tue, 29 Oct 2019 15:03:20 +0900 Subject: [PATCH 27/72] Set param of specific odom frame id --- rois_gazebo/config/robot1_controller.yaml | 1 + rois_gazebo/config/robot2_controller.yaml | 1 + rois_gazebo/config/robot3_controller.yaml | 1 + 3 files changed, 3 insertions(+) diff --git a/rois_gazebo/config/robot1_controller.yaml b/rois_gazebo/config/robot1_controller.yaml index 1b608018..fb8780b1 100644 --- a/rois_gazebo/config/robot1_controller.yaml +++ b/rois_gazebo/config/robot1_controller.yaml @@ -24,6 +24,7 @@ diff_drive_controller: # Base frame_id base_frame_id: robot1/base_footprint #default: base_link + odom_frame_id: robot1/odom # Velocity and acceleration limits # Whenever a min_* is unspecified, default to -max_* diff --git a/rois_gazebo/config/robot2_controller.yaml b/rois_gazebo/config/robot2_controller.yaml index b07792e4..56eabf73 100644 --- a/rois_gazebo/config/robot2_controller.yaml +++ b/rois_gazebo/config/robot2_controller.yaml @@ -24,6 +24,7 @@ diff_drive_controller: # Base frame_id base_frame_id: robot2/base_footprint #default: base_link + odom_frame_id: robot2/odom # Velocity and acceleration limits # Whenever a min_* is unspecified, default to -max_* diff --git a/rois_gazebo/config/robot3_controller.yaml b/rois_gazebo/config/robot3_controller.yaml index ac9d2dbf..e4c01859 100644 --- a/rois_gazebo/config/robot3_controller.yaml +++ b/rois_gazebo/config/robot3_controller.yaml @@ -24,6 +24,7 @@ diff_drive_controller: # Base frame_id base_frame_id: robot3/base_footprint #default: base_link + odom_frame_id: robot3/odom # Velocity and acceleration limits # Whenever a min_* is unspecified, default to -max_* -- GitLab From 98972d4bf27a2a1c808f7016ea26d02e0a9ec33a Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 15:14:46 +0900 Subject: [PATCH 28/72] Modify to read common map --- rois_2dnav_gazebo/config/amcl_params.yaml | 6 +++--- rois_bringup/launch/navigation.launch | 12 ++++++------ 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/rois_2dnav_gazebo/config/amcl_params.yaml b/rois_2dnav_gazebo/config/amcl_params.yaml index eb1cfbd3..be790002 100644 --- a/rois_2dnav_gazebo/config/amcl_params.yaml +++ b/rois_2dnav_gazebo/config/amcl_params.yaml @@ -52,8 +52,8 @@ odom_alpha3: 0.3 odom_alpha4: 0.3 odom_alpha5: 0.1 -odom_frame_id: odom -base_frame_id: base_link -global_frame_id: map +odom_frame_id: robot1/odom +base_frame_id: robot1/base_footprint +global_frame_id: /map tf_broadcast: true diff --git a/rois_bringup/launch/navigation.launch b/rois_bringup/launch/navigation.launch index 832b6898..6dced460 100644 --- a/rois_bringup/launch/navigation.launch +++ b/rois_bringup/launch/navigation.launch @@ -6,6 +6,12 @@ name="use_multi_robot" default="true" /> + + - - -- GitLab From fa69803d97dca4e794f573367e97419f5c40df08 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 15:16:23 +0900 Subject: [PATCH 29/72] Add rviz config for multiple robot navigation --- rois_2dnav_gazebo/rviz/multi_navigation.rviz | 301 +++++++++++++++++++ rois_bringup/launch/navigation.launch | 5 +- 2 files changed, 305 insertions(+), 1 deletion(-) create mode 100644 rois_2dnav_gazebo/rviz/multi_navigation.rviz diff --git a/rois_2dnav_gazebo/rviz/multi_navigation.rviz b/rois_2dnav_gazebo/rviz/multi_navigation.rviz new file mode 100644 index 00000000..6074a332 --- /dev/null +++ b/rois_2dnav_gazebo/rviz/multi_navigation.rviz @@ -0,0 +1,301 @@ +Panels: + - Class: rviz/Displays + Help Height: 85 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /LaserScan1 + - /Map2 + - /Path2 + - /PoseArray1 + Splitter Ratio: 0.29716193675994873 + Tree Height: 801 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.5 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + map: + Value: true + robot1/base_footprint: + Value: true + robot1/base_link: + Value: true + robot1/caster_link: + Value: true + robot1/left_wheel_link: + Value: true + robot1/lrf_link: + Value: true + robot1/odom: + Value: true + robot1/right_wheel_link: + Value: true + Marker Scale: 0.5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + robot1/odom: + robot1/base_footprint: + robot1/base_link: + {} + robot1/caster_link: + {} + robot1/left_wheel_link: + {} + robot1/lrf_link: + {} + robot1/right_wheel_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: /robot1/scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Map + Topic: /move_base/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /move_base/GlobalPlanner/plan + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 52; 101; 164 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /move_base/TrajectoryPlannerROS/local_plan + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: /move_base/local_costmap/footprint + Unreliable: false + Value: true + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: PoseArray + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: /robot1/particlecloud + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 47.22295379638672 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 2.620417356491089 + Y: -0.4015696048736572 + Z: -3.6895456314086914 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.2697967290878296 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.1435768604278564 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1136 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000020a000003bdfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000047000003bd000000f100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002bffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000047000002bf000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039f00000044fc0100000002fb0000000800540069006d006501000000000000039f0000037100fffffffb0000000800540069006d006501000000000000045000000000000000000000039f000003bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 927 + X: 67 + Y: 34 diff --git a/rois_bringup/launch/navigation.launch b/rois_bringup/launch/navigation.launch index 6dced460..1a7c8185 100644 --- a/rois_bringup/launch/navigation.launch +++ b/rois_bringup/launch/navigation.launch @@ -5,6 +5,9 @@ + + value="$(arg rvizconfig)" /> -- GitLab From 56d83160c546af97e4c1a8947f83506b192134ca Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 15:33:52 +0900 Subject: [PATCH 30/72] Set frame_id params for amcl on launch file --- rois_2dnav_gazebo/config/amcl_params.yaml | 4 ---- rois_2dnav_gazebo/launch/amcl.launch | 21 +++++++++++++++++++-- rois_bringup/launch/navigation.launch | 6 +++++- 3 files changed, 24 insertions(+), 7 deletions(-) diff --git a/rois_2dnav_gazebo/config/amcl_params.yaml b/rois_2dnav_gazebo/config/amcl_params.yaml index be790002..dd2fe271 100644 --- a/rois_2dnav_gazebo/config/amcl_params.yaml +++ b/rois_2dnav_gazebo/config/amcl_params.yaml @@ -52,8 +52,4 @@ odom_alpha3: 0.3 odom_alpha4: 0.3 odom_alpha5: 0.1 -odom_frame_id: robot1/odom -base_frame_id: robot1/base_footprint -global_frame_id: /map - tf_broadcast: true diff --git a/rois_2dnav_gazebo/launch/amcl.launch b/rois_2dnav_gazebo/launch/amcl.launch index 7a5e8039..af4bc01c 100644 --- a/rois_2dnav_gazebo/launch/amcl.launch +++ b/rois_2dnav_gazebo/launch/amcl.launch @@ -1,6 +1,23 @@ + + - - + + + + + diff --git a/rois_bringup/launch/navigation.launch b/rois_bringup/launch/navigation.launch index 1a7c8185..dabb326b 100644 --- a/rois_bringup/launch/navigation.launch +++ b/rois_bringup/launch/navigation.launch @@ -25,7 +25,11 @@ - + + + -- GitLab From 12aabfe2e00263ed67d4a81648d7c6284014298d Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 15:47:04 +0900 Subject: [PATCH 31/72] Split param directory for move_base --- .../{ => robot1}/costmap_common_params.yaml | 0 .../{ => robot1}/global_costmap_params.yaml | 0 .../{ => robot1}/local_costmap_params.yaml | 0 .../costmap/robot2/costmap_common_params.yaml | 25 +++++++++++++++++++ .../costmap/robot2/global_costmap_params.yaml | 9 +++++++ .../costmap/robot2/local_costmap_params.yaml | 11 ++++++++ .../costmap/robot3/costmap_common_params.yaml | 25 +++++++++++++++++++ .../costmap/robot3/global_costmap_params.yaml | 9 +++++++ .../costmap/robot3/local_costmap_params.yaml | 11 ++++++++ .../planner/base_local_planner_params.yaml | 2 +- rois_2dnav_gazebo/launch/move_base.launch | 10 +++++--- 11 files changed, 97 insertions(+), 5 deletions(-) rename rois_2dnav_gazebo/config/costmap/{ => robot1}/costmap_common_params.yaml (100%) rename rois_2dnav_gazebo/config/costmap/{ => robot1}/global_costmap_params.yaml (100%) rename rois_2dnav_gazebo/config/costmap/{ => robot1}/local_costmap_params.yaml (100%) create mode 100644 rois_2dnav_gazebo/config/costmap/robot2/costmap_common_params.yaml create mode 100644 rois_2dnav_gazebo/config/costmap/robot2/global_costmap_params.yaml create mode 100644 rois_2dnav_gazebo/config/costmap/robot2/local_costmap_params.yaml create mode 100644 rois_2dnav_gazebo/config/costmap/robot3/costmap_common_params.yaml create mode 100644 rois_2dnav_gazebo/config/costmap/robot3/global_costmap_params.yaml create mode 100644 rois_2dnav_gazebo/config/costmap/robot3/local_costmap_params.yaml diff --git a/rois_2dnav_gazebo/config/costmap/costmap_common_params.yaml b/rois_2dnav_gazebo/config/costmap/robot1/costmap_common_params.yaml similarity index 100% rename from rois_2dnav_gazebo/config/costmap/costmap_common_params.yaml rename to rois_2dnav_gazebo/config/costmap/robot1/costmap_common_params.yaml diff --git a/rois_2dnav_gazebo/config/costmap/global_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot1/global_costmap_params.yaml similarity index 100% rename from rois_2dnav_gazebo/config/costmap/global_costmap_params.yaml rename to rois_2dnav_gazebo/config/costmap/robot1/global_costmap_params.yaml diff --git a/rois_2dnav_gazebo/config/costmap/local_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot1/local_costmap_params.yaml similarity index 100% rename from rois_2dnav_gazebo/config/costmap/local_costmap_params.yaml rename to rois_2dnav_gazebo/config/costmap/robot1/local_costmap_params.yaml diff --git a/rois_2dnav_gazebo/config/costmap/robot2/costmap_common_params.yaml b/rois_2dnav_gazebo/config/costmap/robot2/costmap_common_params.yaml new file mode 100644 index 00000000..63fa6eb7 --- /dev/null +++ b/rois_2dnav_gazebo/config/costmap/robot2/costmap_common_params.yaml @@ -0,0 +1,25 @@ +obstacle_range: 5.0 +raytrace_range: 10.0 + +footprint: [[0.25, -0.18], [0.25, 0.18], [-0.15, 0.18], [-0.15, -0.18]] +inflation_radius: 0.3 +cost_scaling_factor: 10.0 + +origin_z: 0.0 +z_resolution: 0.02 +z_voxels: 0.02 +publish_voxel_map: true + +observation_sources: lrf_sensor +lrf_sensor: { + sensor_frame: lrf_link, + data_type: LaserScan, + topic: /scan, + # expected_update_rate: 1.0, + observation_persistence: 0.0, + marking: true, + clearing: true, + min_obstacle_height: 0.2, + max_obstacle_height: 1.0, +} + diff --git a/rois_2dnav_gazebo/config/costmap/robot2/global_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot2/global_costmap_params.yaml new file mode 100644 index 00000000..e503908e --- /dev/null +++ b/rois_2dnav_gazebo/config/costmap/robot2/global_costmap_params.yaml @@ -0,0 +1,9 @@ +global_costmap: + global_frame: map + robot_base_frame: base_link + update_frequency: 3.0 + publish_frequency: 3.0 + static_map: true + rolling_window: false + transform_tolerance: 1.0 + diff --git a/rois_2dnav_gazebo/config/costmap/robot2/local_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot2/local_costmap_params.yaml new file mode 100644 index 00000000..fe1fd305 --- /dev/null +++ b/rois_2dnav_gazebo/config/costmap/robot2/local_costmap_params.yaml @@ -0,0 +1,11 @@ +local_costmap: + global_frame: odom + robot_base_frame: base_link + update_frequency: 8.0 + publish_frequency: 7.0 + width: 10.0 + height: 10.0 + resolution: 0.02 + meter_scoring: true + static_map: false + rolling_window: true diff --git a/rois_2dnav_gazebo/config/costmap/robot3/costmap_common_params.yaml b/rois_2dnav_gazebo/config/costmap/robot3/costmap_common_params.yaml new file mode 100644 index 00000000..63fa6eb7 --- /dev/null +++ b/rois_2dnav_gazebo/config/costmap/robot3/costmap_common_params.yaml @@ -0,0 +1,25 @@ +obstacle_range: 5.0 +raytrace_range: 10.0 + +footprint: [[0.25, -0.18], [0.25, 0.18], [-0.15, 0.18], [-0.15, -0.18]] +inflation_radius: 0.3 +cost_scaling_factor: 10.0 + +origin_z: 0.0 +z_resolution: 0.02 +z_voxels: 0.02 +publish_voxel_map: true + +observation_sources: lrf_sensor +lrf_sensor: { + sensor_frame: lrf_link, + data_type: LaserScan, + topic: /scan, + # expected_update_rate: 1.0, + observation_persistence: 0.0, + marking: true, + clearing: true, + min_obstacle_height: 0.2, + max_obstacle_height: 1.0, +} + diff --git a/rois_2dnav_gazebo/config/costmap/robot3/global_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot3/global_costmap_params.yaml new file mode 100644 index 00000000..e503908e --- /dev/null +++ b/rois_2dnav_gazebo/config/costmap/robot3/global_costmap_params.yaml @@ -0,0 +1,9 @@ +global_costmap: + global_frame: map + robot_base_frame: base_link + update_frequency: 3.0 + publish_frequency: 3.0 + static_map: true + rolling_window: false + transform_tolerance: 1.0 + diff --git a/rois_2dnav_gazebo/config/costmap/robot3/local_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot3/local_costmap_params.yaml new file mode 100644 index 00000000..fe1fd305 --- /dev/null +++ b/rois_2dnav_gazebo/config/costmap/robot3/local_costmap_params.yaml @@ -0,0 +1,11 @@ +local_costmap: + global_frame: odom + robot_base_frame: base_link + update_frequency: 8.0 + publish_frequency: 7.0 + width: 10.0 + height: 10.0 + resolution: 0.02 + meter_scoring: true + static_map: false + rolling_window: true diff --git a/rois_2dnav_gazebo/config/planner/base_local_planner_params.yaml b/rois_2dnav_gazebo/config/planner/base_local_planner_params.yaml index 669410df..7459a1ab 100644 --- a/rois_2dnav_gazebo/config/planner/base_local_planner_params.yaml +++ b/rois_2dnav_gazebo/config/planner/base_local_planner_params.yaml @@ -26,6 +26,6 @@ TrajectoryPlannerROS: # Show cost map publish_cost_grid_pc: true - global_frame_id: map + global_frame_id: /map meter_scoring: false diff --git a/rois_2dnav_gazebo/launch/move_base.launch b/rois_2dnav_gazebo/launch/move_base.launch index bef6ff64..8f4d72c2 100644 --- a/rois_2dnav_gazebo/launch/move_base.launch +++ b/rois_2dnav_gazebo/launch/move_base.launch @@ -1,12 +1,14 @@ + - - + + - - + + -- GitLab From 38d9028219feafc665dfc48d8c5f3b275efa2495 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 15:54:14 +0900 Subject: [PATCH 32/72] Use robot_name argument for launching move_base --- rois_2dnav_gazebo/launch/move_base.launch | 1 + rois_bringup/launch/navigation.launch | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/rois_2dnav_gazebo/launch/move_base.launch b/rois_2dnav_gazebo/launch/move_base.launch index 8f4d72c2..43808a86 100644 --- a/rois_2dnav_gazebo/launch/move_base.launch +++ b/rois_2dnav_gazebo/launch/move_base.launch @@ -16,5 +16,6 @@ + diff --git a/rois_bringup/launch/navigation.launch b/rois_bringup/launch/navigation.launch index dabb326b..f857a50d 100644 --- a/rois_bringup/launch/navigation.launch +++ b/rois_bringup/launch/navigation.launch @@ -30,6 +30,10 @@ name="robot_name" value="robot1" /> + + + - -- GitLab From 3a48ac85d749198aca3ef288a61dfa41658a9a7f Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 16:11:30 +0900 Subject: [PATCH 33/72] Fix params of move_base for robot-namespace --- .../config/costmap/robot1/costmap_common_params.yaml | 4 ++-- .../config/costmap/robot1/global_costmap_params.yaml | 2 +- .../config/costmap/robot1/local_costmap_params.yaml | 4 ++-- .../config/costmap/robot2/costmap_common_params.yaml | 4 ++-- .../config/costmap/robot2/global_costmap_params.yaml | 2 +- .../config/costmap/robot2/local_costmap_params.yaml | 4 ++-- .../config/costmap/robot3/costmap_common_params.yaml | 4 ++-- .../config/costmap/robot3/global_costmap_params.yaml | 2 +- .../config/costmap/robot3/local_costmap_params.yaml | 4 ++-- rois_2dnav_gazebo/launch/move_base.launch | 4 ++-- 10 files changed, 17 insertions(+), 17 deletions(-) diff --git a/rois_2dnav_gazebo/config/costmap/robot1/costmap_common_params.yaml b/rois_2dnav_gazebo/config/costmap/robot1/costmap_common_params.yaml index 63fa6eb7..54c9ec91 100644 --- a/rois_2dnav_gazebo/config/costmap/robot1/costmap_common_params.yaml +++ b/rois_2dnav_gazebo/config/costmap/robot1/costmap_common_params.yaml @@ -12,9 +12,9 @@ publish_voxel_map: true observation_sources: lrf_sensor lrf_sensor: { - sensor_frame: lrf_link, + sensor_frame: robot1/lrf_link, data_type: LaserScan, - topic: /scan, + topic: robot1/scan, # expected_update_rate: 1.0, observation_persistence: 0.0, marking: true, diff --git a/rois_2dnav_gazebo/config/costmap/robot1/global_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot1/global_costmap_params.yaml index e503908e..f892f558 100644 --- a/rois_2dnav_gazebo/config/costmap/robot1/global_costmap_params.yaml +++ b/rois_2dnav_gazebo/config/costmap/robot1/global_costmap_params.yaml @@ -1,6 +1,6 @@ global_costmap: global_frame: map - robot_base_frame: base_link + robot_base_frame: robot1/base_footprint update_frequency: 3.0 publish_frequency: 3.0 static_map: true diff --git a/rois_2dnav_gazebo/config/costmap/robot1/local_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot1/local_costmap_params.yaml index fe1fd305..d21d3bff 100644 --- a/rois_2dnav_gazebo/config/costmap/robot1/local_costmap_params.yaml +++ b/rois_2dnav_gazebo/config/costmap/robot1/local_costmap_params.yaml @@ -1,6 +1,6 @@ local_costmap: - global_frame: odom - robot_base_frame: base_link + global_frame: robot1/odom + robot_base_frame: robot1/base_footprint update_frequency: 8.0 publish_frequency: 7.0 width: 10.0 diff --git a/rois_2dnav_gazebo/config/costmap/robot2/costmap_common_params.yaml b/rois_2dnav_gazebo/config/costmap/robot2/costmap_common_params.yaml index 63fa6eb7..62ea2115 100644 --- a/rois_2dnav_gazebo/config/costmap/robot2/costmap_common_params.yaml +++ b/rois_2dnav_gazebo/config/costmap/robot2/costmap_common_params.yaml @@ -12,9 +12,9 @@ publish_voxel_map: true observation_sources: lrf_sensor lrf_sensor: { - sensor_frame: lrf_link, + sensor_frame: robot2/lrf_link, data_type: LaserScan, - topic: /scan, + topic: robot2/scan, # expected_update_rate: 1.0, observation_persistence: 0.0, marking: true, diff --git a/rois_2dnav_gazebo/config/costmap/robot2/global_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot2/global_costmap_params.yaml index e503908e..a318075d 100644 --- a/rois_2dnav_gazebo/config/costmap/robot2/global_costmap_params.yaml +++ b/rois_2dnav_gazebo/config/costmap/robot2/global_costmap_params.yaml @@ -1,6 +1,6 @@ global_costmap: global_frame: map - robot_base_frame: base_link + robot_base_frame: robot2/base_footprint update_frequency: 3.0 publish_frequency: 3.0 static_map: true diff --git a/rois_2dnav_gazebo/config/costmap/robot2/local_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot2/local_costmap_params.yaml index fe1fd305..8065ad90 100644 --- a/rois_2dnav_gazebo/config/costmap/robot2/local_costmap_params.yaml +++ b/rois_2dnav_gazebo/config/costmap/robot2/local_costmap_params.yaml @@ -1,6 +1,6 @@ local_costmap: - global_frame: odom - robot_base_frame: base_link + global_frame: robot2/odom + robot_base_frame: robot2/base_footprint update_frequency: 8.0 publish_frequency: 7.0 width: 10.0 diff --git a/rois_2dnav_gazebo/config/costmap/robot3/costmap_common_params.yaml b/rois_2dnav_gazebo/config/costmap/robot3/costmap_common_params.yaml index 63fa6eb7..9940ebdf 100644 --- a/rois_2dnav_gazebo/config/costmap/robot3/costmap_common_params.yaml +++ b/rois_2dnav_gazebo/config/costmap/robot3/costmap_common_params.yaml @@ -12,9 +12,9 @@ publish_voxel_map: true observation_sources: lrf_sensor lrf_sensor: { - sensor_frame: lrf_link, + sensor_frame: robot3/lrf_link, data_type: LaserScan, - topic: /scan, + topic: robot3/scan, # expected_update_rate: 1.0, observation_persistence: 0.0, marking: true, diff --git a/rois_2dnav_gazebo/config/costmap/robot3/global_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot3/global_costmap_params.yaml index e503908e..9f0bd7e5 100644 --- a/rois_2dnav_gazebo/config/costmap/robot3/global_costmap_params.yaml +++ b/rois_2dnav_gazebo/config/costmap/robot3/global_costmap_params.yaml @@ -1,6 +1,6 @@ global_costmap: global_frame: map - robot_base_frame: base_link + robot_base_frame: robot3/base_footprint update_frequency: 3.0 publish_frequency: 3.0 static_map: true diff --git a/rois_2dnav_gazebo/config/costmap/robot3/local_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot3/local_costmap_params.yaml index fe1fd305..2f5a5af9 100644 --- a/rois_2dnav_gazebo/config/costmap/robot3/local_costmap_params.yaml +++ b/rois_2dnav_gazebo/config/costmap/robot3/local_costmap_params.yaml @@ -1,6 +1,6 @@ local_costmap: - global_frame: odom - robot_base_frame: base_link + global_frame: robot3/odom + robot_base_frame: robot3/base_footprint update_frequency: 8.0 publish_frequency: 7.0 width: 10.0 diff --git a/rois_2dnav_gazebo/launch/move_base.launch b/rois_2dnav_gazebo/launch/move_base.launch index 43808a86..a2286be3 100644 --- a/rois_2dnav_gazebo/launch/move_base.launch +++ b/rois_2dnav_gazebo/launch/move_base.launch @@ -15,7 +15,7 @@ - - + + -- GitLab From a8d716717008b6131b73353156eb5efb7d4ba555 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 16:37:04 +0900 Subject: [PATCH 34/72] Fix sensor topic name for obstacle detection --- .../config/costmap/robot1/costmap_common_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rois_2dnav_gazebo/config/costmap/robot1/costmap_common_params.yaml b/rois_2dnav_gazebo/config/costmap/robot1/costmap_common_params.yaml index 54c9ec91..af297510 100644 --- a/rois_2dnav_gazebo/config/costmap/robot1/costmap_common_params.yaml +++ b/rois_2dnav_gazebo/config/costmap/robot1/costmap_common_params.yaml @@ -14,7 +14,7 @@ observation_sources: lrf_sensor lrf_sensor: { sensor_frame: robot1/lrf_link, data_type: LaserScan, - topic: robot1/scan, + topic: scan, # expected_update_rate: 1.0, observation_persistence: 0.0, marking: true, -- GitLab From 1cffa55b5f1bf043da42740729ccb73012c11ea8 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 16:37:44 +0900 Subject: [PATCH 35/72] Read global map topic for move_base --- rois_2dnav_gazebo/launch/move_base.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/rois_2dnav_gazebo/launch/move_base.launch b/rois_2dnav_gazebo/launch/move_base.launch index a2286be3..342ca9ef 100644 --- a/rois_2dnav_gazebo/launch/move_base.launch +++ b/rois_2dnav_gazebo/launch/move_base.launch @@ -16,6 +16,7 @@ + -- GitLab From b576f7d0a17f147534580e43e6a92e9403a83453 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 29 Oct 2019 16:38:03 +0900 Subject: [PATCH 36/72] Upgrade rvix config --- rois_2dnav_gazebo/rviz/multi_navigation.rviz | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rois_2dnav_gazebo/rviz/multi_navigation.rviz b/rois_2dnav_gazebo/rviz/multi_navigation.rviz index 6074a332..eb286fc1 100644 --- a/rois_2dnav_gazebo/rviz/multi_navigation.rviz +++ b/rois_2dnav_gazebo/rviz/multi_navigation.rviz @@ -159,7 +159,7 @@ Visualization Manager: Draw Behind: false Enabled: true Name: Map - Topic: /move_base/local_costmap/costmap + Topic: /robot1/move_base/local_costmap/costmap Unreliable: false Use Timestamp: false Value: true @@ -206,7 +206,7 @@ Visualization Manager: Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 - Topic: /move_base/TrajectoryPlannerROS/local_plan + Topic: /robot1/move_base/GlobalPlanner/plan Unreliable: false Value: true - Alpha: 1 -- GitLab From 3779037e1b4c8ee605126c795d88ddc37c65731e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 30 Oct 2019 00:57:37 +0900 Subject: [PATCH 37/72] Fix params of obstacle-layer for robot2 and robot3 --- .../config/costmap/robot2/costmap_common_params.yaml | 2 +- .../config/costmap/robot3/costmap_common_params.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rois_2dnav_gazebo/config/costmap/robot2/costmap_common_params.yaml b/rois_2dnav_gazebo/config/costmap/robot2/costmap_common_params.yaml index 62ea2115..6db6da4a 100644 --- a/rois_2dnav_gazebo/config/costmap/robot2/costmap_common_params.yaml +++ b/rois_2dnav_gazebo/config/costmap/robot2/costmap_common_params.yaml @@ -14,7 +14,7 @@ observation_sources: lrf_sensor lrf_sensor: { sensor_frame: robot2/lrf_link, data_type: LaserScan, - topic: robot2/scan, + topic: scan, # expected_update_rate: 1.0, observation_persistence: 0.0, marking: true, diff --git a/rois_2dnav_gazebo/config/costmap/robot3/costmap_common_params.yaml b/rois_2dnav_gazebo/config/costmap/robot3/costmap_common_params.yaml index 9940ebdf..119355e6 100644 --- a/rois_2dnav_gazebo/config/costmap/robot3/costmap_common_params.yaml +++ b/rois_2dnav_gazebo/config/costmap/robot3/costmap_common_params.yaml @@ -14,7 +14,7 @@ observation_sources: lrf_sensor lrf_sensor: { sensor_frame: robot3/lrf_link, data_type: LaserScan, - topic: robot3/scan, + topic: scan, # expected_update_rate: 1.0, observation_persistence: 0.0, marking: true, -- GitLab From 9b35d285f061551550721aad2034eee2554b04e9 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 30 Oct 2019 00:59:17 +0900 Subject: [PATCH 38/72] Launch amcl and move_base for oher robots --- rois_bringup/launch/navigation.launch | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/rois_bringup/launch/navigation.launch b/rois_bringup/launch/navigation.launch index f857a50d..49977f6f 100644 --- a/rois_bringup/launch/navigation.launch +++ b/rois_bringup/launch/navigation.launch @@ -36,4 +36,30 @@ value="robot1" /> + + + + + + + + + + + + + + + + + + -- GitLab From 7af9cf08369e57afc57bc09a0cb2e21a2e12ed2f Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 30 Oct 2019 02:12:24 +0900 Subject: [PATCH 39/72] Set init pos config for amcl --- rois_2dnav_gazebo/launch/amcl.launch | 12 ++++++++++++ rois_bringup/launch/navigation.launch | 12 ++++++++++++ 2 files changed, 24 insertions(+) diff --git a/rois_2dnav_gazebo/launch/amcl.launch b/rois_2dnav_gazebo/launch/amcl.launch index af4bc01c..9ae6cbba 100644 --- a/rois_2dnav_gazebo/launch/amcl.launch +++ b/rois_2dnav_gazebo/launch/amcl.launch @@ -2,6 +2,12 @@ + + + + diff --git a/rois_bringup/launch/navigation.launch b/rois_bringup/launch/navigation.launch index 49977f6f..6d2981a9 100644 --- a/rois_bringup/launch/navigation.launch +++ b/rois_bringup/launch/navigation.launch @@ -42,6 +42,12 @@ + + + + Date: Wed, 30 Oct 2019 14:41:53 +0900 Subject: [PATCH 40/72] Add argument of port and robot_name on Navigation component --- goal_sender/scripts/Navigation.py | 17 +++++++++++------ goal_sender/scripts/goal_sender_ros.py | 4 ++-- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/goal_sender/scripts/Navigation.py b/goal_sender/scripts/Navigation.py index 879b2bdf..fb38f75c 100755 --- a/goal_sender/scripts/Navigation.py +++ b/goal_sender/scripts/Navigation.py @@ -76,10 +76,10 @@ class Event(RoIS_Common.Event): from goal_sender_ros import GoalSenderROS class Navigation(Event, Command, Query): - def __init__(self, c): + def __init__(self, c, robot_name): super(Navigation, self).__init__(c) self._component = c - self._goal_sender = GoalSenderROS() + self._goal_sender = GoalSenderROS(robot_name) self._component.Target_Position = [""] self._component.Time_Limit = 10 self._component.Routing_Policy = "" @@ -106,10 +106,10 @@ class Component(object): import rospy -def example_n(port): +def example_n(port, robot_name): print("Starting node...") component = Component() - n = Navigation(component) + n = Navigation(component, robot_name) print("Navigation constructed") # start the timer to dispatch events @@ -125,5 +125,10 @@ def example_n(port): if __name__ == '__main__': - rospy.init_node('navigation_component') - example_n(8001) + if len(sys.argv) < 3: + print("rosrun Navigation.py ") + raise RuntimeError + + port, robot_name = int(sys.argv[1]), sys.argv[2] + rospy.init_node('navigation_component', anonymous=True) + example_n(port=port, robot_name=robot_name) diff --git a/goal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py index c6058e48..3b69730d 100644 --- a/goal_sender/scripts/goal_sender_ros.py +++ b/goal_sender/scripts/goal_sender_ros.py @@ -38,8 +38,8 @@ class GoalSenderROS(object): g_state.LOST: c_state.ERROR } - def __init__(self): - self.action_client = actionlib.SimpleActionClient('move_base', MoveBaseAction) + def __init__(self, robot_name): + self.action_client = actionlib.SimpleActionClient(robot_name+'/move_base', MoveBaseAction) self.action_client.wait_for_server() def send_goal(self, pose_xy): -- GitLab From 1ce4543dcbb5ebb3f23d59a0fcb9076769b67b0e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 30 Oct 2019 16:38:21 +0900 Subject: [PATCH 41/72] Modify engine for dynamic port number --- goal_sender/scripts/route_guidance_engine.py | 15 ++++++++++----- goal_sender/scripts/test_engine.py | 4 ++-- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py index ab821d0a..87d1f049 100644 --- a/goal_sender/scripts/route_guidance_engine.py +++ b/goal_sender/scripts/route_guidance_engine.py @@ -171,11 +171,11 @@ from Navigation_client import Navigation_Client as NavClient class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application_Base): """IF """ - def __init__(self, Engine): + def __init__(self, Engine, nav_port): super().__init__(Engine) self.command_id = 0 self.compoent_clients = { - 'Navigation': NavClient('http://localhost:8001') + 'Navigation': NavClient('http://localhost:' + nav_port) } def set_parameter(self, component_ref, parameters): @@ -218,11 +218,16 @@ class RouteGuidanceEngine: self.state = False -def test_engine(port): +def test_engine(port, nav_port): """test_engine """ - IF_server(port).run(IF(RouteGuidanceEngine())) + IF_server(port).run(IF(RouteGuidanceEngine(), nav_port)) if __name__ == "__main__": - test_engine(8010) + if len(sys.argv) < 3: + print("py route_guidance_engine.py ") + raise RuntimeError + + port, nav_port = int(sys.argv[1]), sys.argv[2] + test_engine(port, nav_port) diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py index a0e913f9..fc5295a0 100644 --- a/goal_sender/scripts/test_engine.py +++ b/goal_sender/scripts/test_engine.py @@ -1,6 +1,6 @@ # 1. launch navigation. -# 2. execute "rosrun (this_package) Navigation.py". -# 3. run route_guidance_engine server. +# 2. execute "rosrun (this_package) Navigation.py 8001 robot1". +# 3. run route_guidance_engine server with server and component port. # 4. run this test. -- GitLab From f207bed11af402067b2840e8a7285d528620eddb Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 30 Oct 2019 17:41:30 +0900 Subject: [PATCH 42/72] Rename route_guidance_engine* => sub_\1 --- ...oute_guidance_engine.py => sub_route_guidance_engine.py} | 6 +++--- ...engine_client.py => sub_route_guidance_engine_client.py} | 0 goal_sender/scripts/{test_engine.py => test_sub_engine.py} | 4 ++-- goal_sender/scripts/test_toplevel_engine.py | 2 +- goal_sender/scripts/toplevel_route_guidance_engine.py | 2 +- 5 files changed, 7 insertions(+), 7 deletions(-) rename goal_sender/scripts/{route_guidance_engine.py => sub_route_guidance_engine.py} (97%) rename goal_sender/scripts/{route_guidance_engine_client.py => sub_route_guidance_engine_client.py} (100%) rename goal_sender/scripts/{test_engine.py => test_sub_engine.py} (88%) diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/sub_route_guidance_engine.py similarity index 97% rename from goal_sender/scripts/route_guidance_engine.py rename to goal_sender/scripts/sub_route_guidance_engine.py index 87d1f049..53995591 100644 --- a/goal_sender/scripts/route_guidance_engine.py +++ b/goal_sender/scripts/sub_route_guidance_engine.py @@ -1,4 +1,4 @@ -# route_guidance_engine.py +# sub_route_guidance_engine.py # # Copyright 2017 Eiichi Inohira # This software may be modified and distributed under the terms @@ -7,7 +7,7 @@ # For python 3 # For HRI Engine -"""route_guidance_engine +"""sub_route_guidance_engine """ from __future__ import print_function @@ -226,7 +226,7 @@ def test_engine(port, nav_port): if __name__ == "__main__": if len(sys.argv) < 3: - print("py route_guidance_engine.py ") + print("py sub_route_guidance_engine.py ") raise RuntimeError port, nav_port = int(sys.argv[1]), sys.argv[2] diff --git a/goal_sender/scripts/route_guidance_engine_client.py b/goal_sender/scripts/sub_route_guidance_engine_client.py similarity index 100% rename from goal_sender/scripts/route_guidance_engine_client.py rename to goal_sender/scripts/sub_route_guidance_engine_client.py diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_sub_engine.py similarity index 88% rename from goal_sender/scripts/test_engine.py rename to goal_sender/scripts/test_sub_engine.py index fc5295a0..bce26dbc 100644 --- a/goal_sender/scripts/test_engine.py +++ b/goal_sender/scripts/test_sub_engine.py @@ -1,10 +1,10 @@ # 1. launch navigation. # 2. execute "rosrun (this_package) Navigation.py 8001 robot1". -# 3. run route_guidance_engine server with server and component port. +# 3. run sub_route_guidance_engine server with server and component port. # 4. run this test. -from route_guidance_engine_client import IF +from sub_route_guidance_engine_client import IF import time from pyrois.RoIS_Common import Component_Status diff --git a/goal_sender/scripts/test_toplevel_engine.py b/goal_sender/scripts/test_toplevel_engine.py index e57fb1ba..407a75d2 100644 --- a/goal_sender/scripts/test_toplevel_engine.py +++ b/goal_sender/scripts/test_toplevel_engine.py @@ -1,6 +1,6 @@ # 1. launch navigation. # 2. execute "rosrun (this_package) Navigation.py". -# 3. run route_guidance_engine server. +# 3. run sub_route_guidance_engine server. # 4. run toplevel_route_guidance_engine server. # 5. run this test. diff --git a/goal_sender/scripts/toplevel_route_guidance_engine.py b/goal_sender/scripts/toplevel_route_guidance_engine.py index db0043b3..2b395108 100644 --- a/goal_sender/scripts/toplevel_route_guidance_engine.py +++ b/goal_sender/scripts/toplevel_route_guidance_engine.py @@ -164,7 +164,7 @@ class EventIF(RoIS_HRI.EventIF): results = ["None"] return (status, results) -from route_guidance_engine_client import IF as EngineClient +from sub_route_guidance_engine_client import IF as EngineClient class IF(SystemIF, CommandIF, QueryIF, EventIF): """IF -- GitLab From ef6550152c38c69d879631034e082cd52db4e2c6 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 30 Oct 2019 18:53:37 +0900 Subject: [PATCH 43/72] Add engine clients for robot2 and robot3 on TopLevelEngine --- goal_sender/scripts/toplevel_route_guidance_engine.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/goal_sender/scripts/toplevel_route_guidance_engine.py b/goal_sender/scripts/toplevel_route_guidance_engine.py index 2b395108..95e4aa98 100644 --- a/goal_sender/scripts/toplevel_route_guidance_engine.py +++ b/goal_sender/scripts/toplevel_route_guidance_engine.py @@ -173,7 +173,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): super().__init__(Engine) self.command_id = 0 self.engine_clients = { - 'robot1': EngineClient('http://localhost:8010') + 'robot1': EngineClient('http://localhost:8010'), + 'robot2': EngineClient('http://localhost:8020'), + 'robot3': EngineClient('http://localhost:8030') } for engine_client in self.engine_clients.values(): engine_client.connect() -- GitLab From 6371874d8cd37c0d864319ac1e830d20c904c46d Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 31 Oct 2019 13:41:02 +0900 Subject: [PATCH 44/72] Upgade GoalSender to subscribe amcl_pose --- goal_sender/scripts/goal_sender_ros.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/goal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py index 3b69730d..8b5a36bd 100644 --- a/goal_sender/scripts/goal_sender_ros.py +++ b/goal_sender/scripts/goal_sender_ros.py @@ -4,7 +4,7 @@ import rospy import actionlib from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped from actionlib_msgs.msg import GoalStatus as g_state from pyrois.RoIS_Common import Component_Status as c_state @@ -40,6 +40,7 @@ class GoalSenderROS(object): def __init__(self, robot_name): self.action_client = actionlib.SimpleActionClient(robot_name+'/move_base', MoveBaseAction) + rospy.Subscriber(robot_name+'/amcl_pose', PoseWithCovarianceStamped, self.amcl_pose_callback) self.action_client.wait_for_server() def send_goal(self, pose_xy): @@ -52,5 +53,8 @@ class GoalSenderROS(object): state = self.action_client.get_state() return GoalSenderROS.GoalSenderState[state] + def amcl_pose_callback(self, amcl_pose): + print(amcl_pose.pose.pose.position) + # If you remove this comment out, it does not work. # rospy.init_node('goal_sender') -- GitLab From e2cffb4deccc90fbc3a706259ad2e3c2c64478f7 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 1 Nov 2019 00:35:41 +0900 Subject: [PATCH 45/72] Add function to get robot's current position --- goal_sender/scripts/goal_sender_ros.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/goal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py index 8b5a36bd..801530ab 100644 --- a/goal_sender/scripts/goal_sender_ros.py +++ b/goal_sender/scripts/goal_sender_ros.py @@ -40,6 +40,8 @@ class GoalSenderROS(object): def __init__(self, robot_name): self.action_client = actionlib.SimpleActionClient(robot_name+'/move_base', MoveBaseAction) + self.position = None + rospy.Subscriber(robot_name+'/amcl_pose', PoseWithCovarianceStamped, self.amcl_pose_callback) self.action_client.wait_for_server() @@ -54,7 +56,11 @@ class GoalSenderROS(object): return GoalSenderROS.GoalSenderState[state] def amcl_pose_callback(self, amcl_pose): - print(amcl_pose.pose.pose.position) + pos = amcl_pose.pose.pose.position + self.position = [pos.x, pos.y] + + def get_current_position(self): + return self.position # If you remove this comment out, it does not work. # rospy.init_node('goal_sender') -- GitLab From 5903eaec3ebd3e765d26e827f1cc0e1964166315 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 1 Nov 2019 01:32:29 +0900 Subject: [PATCH 46/72] Add System_Information compoent files --- goal_sender/scripts/System_Information.py | 113 ++++++++++++++++++ .../scripts/System_Information_client.py | 41 +++++++ 2 files changed, 154 insertions(+) create mode 100755 goal_sender/scripts/System_Information.py create mode 100644 goal_sender/scripts/System_Information_client.py diff --git a/goal_sender/scripts/System_Information.py b/goal_sender/scripts/System_Information.py new file mode 100755 index 00000000..e3676c82 --- /dev/null +++ b/goal_sender/scripts/System_Information.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python + +# System_Information.py +# +# Copyright 2018 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python3 +# For HRI Component + +"""System_Information +""" + +import sys +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): + """ThreadingXMLRPCServer + """ + pass + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, c): + super(Query, self).__init__() + self._component = c + + def component_status(self): + pass + + def robot_position(self): + pass + + def engine_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + e_status = RoIS_Common.Component_Status.READY.value + operable_time = ["2100-01-01T00:00:01+09:00","2100-01-01T00:00:01+09:00"] + return (status, e_status, operable_time) + + +class System_Information(Query): + """System_Information + """ + def __init__(self, c): + super(System_Information, self).__init__(c) + self._component = c + + def robot_position(self): + status = RoIS_HRI.ReturnCode_t.OK.value + timestamp = str(datetime.now()) + robot_ref = ["urn:x-rois:def:HRIComponent:Kyutech::robot1"] # codebook, version + position_data = ["0", "0"] + return(status, timestamp, robot_ref, position_data) + + +import rospy + +class component: + """component + """ + def __init__(self, robot_name): + self._state = False + + +def event_dispatch(pd): + """event_dispatch + """ + +def example_si(port, robot_name): + """example_si + """ + c = component(robot_name) + si = System_Information(c) + + # start the timer to dispatch events + t = threading.Timer(0.1, event_dispatch, args=(si,)) + t.start() + + # start the XML-RPC server + server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) + server.register_instance(si) + server.register_introspection_functions() + server.register_multicall_functions() + server.serve_forever() + + +if __name__ == '__main__': + if len(sys.argv) < 3: + print("rosrun System_Information.py ") + raise RuntimeError + + port, robot_name = int(sys.argv[1]), sys.argv[2] + rospy.init_node('sysinfo_component', anonymous=True) + example_si(port, robot_name) diff --git a/goal_sender/scripts/System_Information_client.py b/goal_sender/scripts/System_Information_client.py new file mode 100644 index 00000000..7285134d --- /dev/null +++ b/goal_sender/scripts/System_Information_client.py @@ -0,0 +1,41 @@ +# System_Information_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 + +"""System_Information_Client +""" + +import threading +# import logging +import xmlrpc.client + +from pyrois import RoIS_Common, RoIS_HRI + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def robot_position(self): + (status, timestamp, robot_ref, position_data) = self._proxy.component_status() + return(RoIS_HRI.ReturnCode_t(status), timestamp, robot_ref, position_data) + + def engine_status(self): + (status, e_status, operable_time) = self._proxy.component_status() + return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(e_status), operable_time) + + +class System_Information_Client(Query): + """System_Information_Client + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) \ No newline at end of file -- GitLab From 9c0fc3a24fd0260680a5a05ec2b7719168225e7a Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 1 Nov 2019 02:11:53 +0900 Subject: [PATCH 47/72] Upgrade SysInfor component with amcl_pose subscriber --- goal_sender/scripts/System_Information.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/goal_sender/scripts/System_Information.py b/goal_sender/scripts/System_Information.py index e3676c82..908384b1 100755 --- a/goal_sender/scripts/System_Information.py +++ b/goal_sender/scripts/System_Information.py @@ -68,18 +68,27 @@ class System_Information(Query): status = RoIS_HRI.ReturnCode_t.OK.value timestamp = str(datetime.now()) robot_ref = ["urn:x-rois:def:HRIComponent:Kyutech::robot1"] # codebook, version - position_data = ["0", "0"] + position_data = self._component.get_position() return(status, timestamp, robot_ref, position_data) import rospy +from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped class component: """component """ def __init__(self, robot_name): self._state = False + self.position = None + rospy.Subscriber(robot_name+'/amcl_pose', PoseWithCovarianceStamped, self.amcl_pose_callback) + def amcl_pose_callback(self, amcl_pose): + pos = amcl_pose.pose.pose.position + self.position = [pos.x, pos.y] + + def get_position(self): + return self.position def event_dispatch(pd): """event_dispatch -- GitLab From aaaaf80cab69e12d6bfb3c91de07c476e03aae3d Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 1 Nov 2019 02:13:18 +0900 Subject: [PATCH 48/72] Remove amcl_pose subscriber from goal_sender --- goal_sender/scripts/goal_sender_ros.py | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/goal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py index 801530ab..3b69730d 100644 --- a/goal_sender/scripts/goal_sender_ros.py +++ b/goal_sender/scripts/goal_sender_ros.py @@ -4,7 +4,7 @@ import rospy import actionlib from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped +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 @@ -40,9 +40,6 @@ class GoalSenderROS(object): def __init__(self, robot_name): self.action_client = actionlib.SimpleActionClient(robot_name+'/move_base', MoveBaseAction) - self.position = None - - rospy.Subscriber(robot_name+'/amcl_pose', PoseWithCovarianceStamped, self.amcl_pose_callback) self.action_client.wait_for_server() def send_goal(self, pose_xy): @@ -55,12 +52,5 @@ class GoalSenderROS(object): state = self.action_client.get_state() return GoalSenderROS.GoalSenderState[state] - def amcl_pose_callback(self, amcl_pose): - pos = amcl_pose.pose.pose.position - self.position = [pos.x, pos.y] - - def get_current_position(self): - return self.position - # If you remove this comment out, it does not work. # rospy.init_node('goal_sender') -- GitLab From 2b69c1ac32ff872ca6a3b2f9aa6eb790a857bae6 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 1 Nov 2019 02:32:30 +0900 Subject: [PATCH 49/72] Include System_Information client into sub_engine --- .../scripts/sub_route_guidance_engine.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/goal_sender/scripts/sub_route_guidance_engine.py b/goal_sender/scripts/sub_route_guidance_engine.py index 53995591..e0533a84 100644 --- a/goal_sender/scripts/sub_route_guidance_engine.py +++ b/goal_sender/scripts/sub_route_guidance_engine.py @@ -167,15 +167,17 @@ class EventIF(RoIS_HRI.EventIF): from Navigation_client import Navigation_Client as NavClient +from System_Information_client import System_Information_Client as SysClient class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application_Base): """IF """ - def __init__(self, Engine, nav_port): + def __init__(self, Engine, engine_port): super().__init__(Engine) self.command_id = 0 self.compoent_clients = { - 'Navigation': NavClient('http://localhost:' + nav_port) + 'Navigation': NavClient('http://localhost:' + str(engine_port+1)), + 'System_Information': SysClient('http://localhost:' + str(engine_port+2)) } def set_parameter(self, component_ref, parameters): @@ -218,16 +220,16 @@ class RouteGuidanceEngine: self.state = False -def test_engine(port, nav_port): +def test_engine(port): """test_engine """ - IF_server(port).run(IF(RouteGuidanceEngine(), nav_port)) + IF_server(port).run(IF(RouteGuidanceEngine(), port)) if __name__ == "__main__": - if len(sys.argv) < 3: - print("py sub_route_guidance_engine.py ") + if len(sys.argv) < 2: + print("py sub_route_guidance_engine.py ") raise RuntimeError - port, nav_port = int(sys.argv[1]), sys.argv[2] - test_engine(port, nav_port) + port = int(sys.argv[1]) + test_engine(port) -- GitLab From d2916579084000cabbfd293fef68cbd540c3b3eb Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 1 Nov 2019 13:19:34 +0900 Subject: [PATCH 50/72] Add test for System_Information component --- .../{test_component.py => test_nav_component.py} | 4 ++-- goal_sender/scripts/test_sys_component.py | 13 +++++++++++++ 2 files changed, 15 insertions(+), 2 deletions(-) rename goal_sender/scripts/{test_component.py => test_nav_component.py} (70%) create mode 100644 goal_sender/scripts/test_sys_component.py diff --git a/goal_sender/scripts/test_component.py b/goal_sender/scripts/test_nav_component.py similarity index 70% rename from goal_sender/scripts/test_component.py rename to goal_sender/scripts/test_nav_component.py index 25e4a77e..66fb8949 100644 --- a/goal_sender/scripts/test_component.py +++ b/goal_sender/scripts/test_nav_component.py @@ -1,11 +1,11 @@ # 1. launch navigation. -# 2. execute "rosrun (this_package) Navigation.py" +# 2. execute "rosrun (this_package) Navigation.py 8011 robot1" # 3. run this test import time from Navigation_client import Navigation_Client -uri = "http://localhost:8001" +uri = "http://localhost:8011" client = Navigation_Client(uri) print(client.start()) diff --git a/goal_sender/scripts/test_sys_component.py b/goal_sender/scripts/test_sys_component.py new file mode 100644 index 00000000..223bdb63 --- /dev/null +++ b/goal_sender/scripts/test_sys_component.py @@ -0,0 +1,13 @@ +# 1. launch navigation. +# 2. execute "rosrun (this_package) System_Information.py 8012 robot1" +# 3. run this test + +import time +from System_Information_client import System_Information_Client + +uri = "http://localhost:8012" +client = System_Information_Client(uri) + +while True: + time.sleep(1) + print(client.robot_position()) -- GitLab From 9f996e020f24e4c4d41be7533c41706509cf4cb3 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 1 Nov 2019 13:42:45 +0900 Subject: [PATCH 51/72] Fix bug on System_Information_client --- goal_sender/scripts/System_Information.py | 2 +- goal_sender/scripts/System_Information_client.py | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/goal_sender/scripts/System_Information.py b/goal_sender/scripts/System_Information.py index 908384b1..a2493cc1 100755 --- a/goal_sender/scripts/System_Information.py +++ b/goal_sender/scripts/System_Information.py @@ -69,7 +69,7 @@ class System_Information(Query): timestamp = str(datetime.now()) robot_ref = ["urn:x-rois:def:HRIComponent:Kyutech::robot1"] # codebook, version position_data = self._component.get_position() - return(status, timestamp, robot_ref, position_data) + return (status, timestamp, robot_ref, position_data) import rospy diff --git a/goal_sender/scripts/System_Information_client.py b/goal_sender/scripts/System_Information_client.py index 7285134d..d44ddc10 100644 --- a/goal_sender/scripts/System_Information_client.py +++ b/goal_sender/scripts/System_Information_client.py @@ -25,8 +25,8 @@ class Query(RoIS_Common.Query): self._proxy = xmlrpc.client.ServerProxy(self._uri) def robot_position(self): - (status, timestamp, robot_ref, position_data) = self._proxy.component_status() - return(RoIS_HRI.ReturnCode_t(status), timestamp, robot_ref, position_data) + (status, timestamp, robot_ref, position_data) = self._proxy.robot_position() + return (RoIS_HRI.ReturnCode_t(status), timestamp, robot_ref, position_data) def engine_status(self): (status, e_status, operable_time) = self._proxy.component_status() @@ -38,4 +38,4 @@ class System_Information_Client(Query): """ def __init__(self, uri): self._uri = uri - self._proxy = xmlrpc.client.ServerProxy(self._uri) \ No newline at end of file + self._proxy = xmlrpc.client.ServerProxy(self._uri) -- GitLab From 8864752a2f3ed3b4e55ad4260c4b0499f2711ad8 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 1 Nov 2019 18:16:25 +0900 Subject: [PATCH 52/72] Inherit Service_Application_IF for HRI engine client --- goal_sender/scripts/sub_route_guidance_engine_client.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/goal_sender/scripts/sub_route_guidance_engine_client.py b/goal_sender/scripts/sub_route_guidance_engine_client.py index 6abe2d97..6e9267ff 100644 --- a/goal_sender/scripts/sub_route_guidance_engine_client.py +++ b/goal_sender/scripts/sub_route_guidance_engine_client.py @@ -14,6 +14,7 @@ import xmlrpc.client # from pyrois import Service_Application_Base_example from pyrois import RoIS_HRI, RoIS_Common, RoIS_Service +from pyrois.Service_Application_IF import Service_Application_IF # import Service_Application_IF_test class SystemIF(RoIS_HRI.SystemIF): @@ -120,7 +121,7 @@ class EventIF(RoIS_HRI.EventIF): return (status, results) -class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application_Base): +class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_IF): """IF """ def __init__(self, uri): -- GitLab From d693fbdeaab8602c5649cf01fde336639185c021 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 3 Nov 2019 15:10:43 +0900 Subject: [PATCH 53/72] Refactor engine test --- goal_sender/scripts/test_sub_engine.py | 20 ++++++++------------ goal_sender/scripts/test_toplevel_engine.py | 11 +++++------ 2 files changed, 13 insertions(+), 18 deletions(-) diff --git a/goal_sender/scripts/test_sub_engine.py b/goal_sender/scripts/test_sub_engine.py index bce26dbc..a884cfbc 100644 --- a/goal_sender/scripts/test_sub_engine.py +++ b/goal_sender/scripts/test_sub_engine.py @@ -4,27 +4,23 @@ # 4. run this test. -from sub_route_guidance_engine_client import IF +from sub_route_guidance_engine_client import Engine import time from pyrois.RoIS_Common import Component_Status -s = 'http://127.0.0.1:8010' -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') +engine = Engine('http://127.0.0.1:8010') +engine.connect() +engine.search("engine_profile_test") destinations = [[5, 0], [10, 0], [25, 0], [25, 10]] for dest in destinations: - print(i.set_parameter('Navigation',[dest, "", ""])) + print(engine.set_parameter('Navigation',[dest, "", ""])) navi_status = Component_Status.BUSY - print("TEST: status = {}".format(navi_status)) while navi_status != Component_Status.READY: - server_status, navi_status = i.get_navi_status() + server_status, navi_status = engine.get_navi_status() print("TEST: status = {}".format(navi_status)) time.sleep(1) time.sleep(3) -#print(i.analysis_c_status()) -i.disconnect() +#print(engine.analysis_c_status()) +engine.disconnect() print("finish") diff --git a/goal_sender/scripts/test_toplevel_engine.py b/goal_sender/scripts/test_toplevel_engine.py index 407a75d2..3d7291ac 100644 --- a/goal_sender/scripts/test_toplevel_engine.py +++ b/goal_sender/scripts/test_toplevel_engine.py @@ -5,16 +5,15 @@ # 5. run this test. -from toplevel_route_guidance_engine_client import IF +from toplevel_route_guidance_engine_client import Engine import time from pyrois.RoIS_Common import Component_Status -s = 'http://127.0.0.1:8000' -i = IF(s) -i.connect() +engine = IF('http://127.0.0.1:8000') +engine.connect() time.sleep(3) -status = i.set_parameter('Navigation',[[25, 20], "", ""]) +status = engine.set_parameter('Navigation',[[25, 20], "", ""]) print(status) -i.disconnect() +engine.disconnect() print("finish") -- GitLab From dca06fffe81026377ad8afd313ae009f15efcbec Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 4 Nov 2019 22:01:44 +0900 Subject: [PATCH 54/72] Add function to get position on sub-engine --- goal_sender/scripts/sub_route_guidance_engine.py | 5 +++++ goal_sender/scripts/sub_route_guidance_engine_client.py | 5 +++++ goal_sender/scripts/test_sub_engine.py | 4 +++- 3 files changed, 13 insertions(+), 1 deletion(-) diff --git a/goal_sender/scripts/sub_route_guidance_engine.py b/goal_sender/scripts/sub_route_guidance_engine.py index e0533a84..33cf9ca9 100644 --- a/goal_sender/scripts/sub_route_guidance_engine.py +++ b/goal_sender/scripts/sub_route_guidance_engine.py @@ -194,6 +194,11 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application def get_navi_status(self): status, c_status = self.compoent_clients['Navigation'].component_status() return (status.value, c_status.value) + + def get_position(self): + si_compoent = self.compoent_clients['System_Information'] + status, timestamp, robot_ref, position_data = si_compoent.robot_position() + return (status.value, timestamp, robot_ref, position_data) ############## class IF_server: diff --git a/goal_sender/scripts/sub_route_guidance_engine_client.py b/goal_sender/scripts/sub_route_guidance_engine_client.py index 6e9267ff..0fc6e7ec 100644 --- a/goal_sender/scripts/sub_route_guidance_engine_client.py +++ b/goal_sender/scripts/sub_route_guidance_engine_client.py @@ -139,4 +139,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_IF): 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) + + def get_position(self): + position = self._proxy.get_position() + position[0] = RoIS_HRI.ReturnCode_t(position[0]) + return position ############## diff --git a/goal_sender/scripts/test_sub_engine.py b/goal_sender/scripts/test_sub_engine.py index a884cfbc..7932218e 100644 --- a/goal_sender/scripts/test_sub_engine.py +++ b/goal_sender/scripts/test_sub_engine.py @@ -4,7 +4,7 @@ # 4. run this test. -from sub_route_guidance_engine_client import Engine +from sub_route_guidance_engine_client import IF as Engine import time from pyrois.RoIS_Common import Component_Status @@ -19,6 +19,8 @@ for dest in destinations: server_status, navi_status = engine.get_navi_status() print("TEST: status = {}".format(navi_status)) time.sleep(1) + position = engine.get_position() + print("TEST: position = {}".format(position)) time.sleep(3) #print(engine.analysis_c_status()) -- GitLab From 102471e29eeea5787a8fbe43dde26282fcb30352 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 00:46:28 +0900 Subject: [PATCH 55/72] Upgrade top-level engine and test for efficient navi --- goal_sender/scripts/test_toplevel_engine.py | 11 ++++++----- .../scripts/toplevel_route_guidance_engine.py | 15 +++++++++++++-- 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/goal_sender/scripts/test_toplevel_engine.py b/goal_sender/scripts/test_toplevel_engine.py index 3d7291ac..f9589007 100644 --- a/goal_sender/scripts/test_toplevel_engine.py +++ b/goal_sender/scripts/test_toplevel_engine.py @@ -5,15 +5,16 @@ # 5. run this test. -from toplevel_route_guidance_engine_client import Engine +from toplevel_route_guidance_engine_client import IF as Engine import time from pyrois.RoIS_Common import Component_Status -engine = IF('http://127.0.0.1:8000') +engine = Engine('http://127.0.0.1:8000') engine.connect() -time.sleep(3) -status = engine.set_parameter('Navigation',[[25, 20], "", ""]) -print(status) +for dest in [[25, 0], [0, 20], [5, 0]]: + status = engine.set_parameter('Navigation', [dest, "", ""]) + print(status) + time.sleep(5) engine.disconnect() print("finish") diff --git a/goal_sender/scripts/toplevel_route_guidance_engine.py b/goal_sender/scripts/toplevel_route_guidance_engine.py index 95e4aa98..aa5aa1e7 100644 --- a/goal_sender/scripts/toplevel_route_guidance_engine.py +++ b/goal_sender/scripts/toplevel_route_guidance_engine.py @@ -14,6 +14,7 @@ from __future__ import print_function import os import sys +import numpy as np from pyrois import RoIS_HRI @@ -181,8 +182,18 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): engine_client.connect() def set_parameter(self, command_ref, parameters): - for engine_client in self.engine_clients.values(): - engine_client.set_parameter(command_ref, parameters) + dest = np.array(parameters[0]) + robot_distance_table = {} + for name, engine in self.engine_clients.items(): + pos = engine.get_position()[3] + print("OK1") + pos = np.array(pos) + robot_distance_table[name] = np.linalg.norm(dest - pos) + nearest_robot = min(robot_distance_table, key=robot_distance_table.get) + print("Nearest robot is '{}'".format(nearest_robot)) + + target_engine = self.engine_clients[nearest_robot] + target_engine.set_parameter(command_ref, parameters) status = RoIS_HRI.ReturnCode_t.OK.value self.command_id += 1 return (status, str(self.command_id)) -- GitLab From d22fca035b261658389a441d93cc3a3648132e14 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 01:50:55 +0900 Subject: [PATCH 56/72] Upgrade test script to avoid to create many windows --- goal_sender/scripts/test_toplevel_engine.py | 20 ++++++++++++++++++++ goal_sender/scripts/utilities.py | 19 +++++++++++++++++++ 2 files changed, 39 insertions(+) create mode 100644 goal_sender/scripts/utilities.py diff --git a/goal_sender/scripts/test_toplevel_engine.py b/goal_sender/scripts/test_toplevel_engine.py index f9589007..a963bcf9 100644 --- a/goal_sender/scripts/test_toplevel_engine.py +++ b/goal_sender/scripts/test_toplevel_engine.py @@ -8,6 +8,26 @@ from toplevel_route_guidance_engine_client import IF as Engine import time from pyrois.RoIS_Common import Component_Status +from utilities import NavigationComponent, SysInfoComponent +from subprocess import Popen + +robot_port_table = {'robot1': 8010, 'robot2': 8020, 'robot3': 8030} +components = [] +for robot_name, engine_port in robot_port_table.items(): + components.append(NavigationComponent(robot_name, engine_port+1)) + components.append(SysInfoComponent(robot_name, engine_port+2)) +print("Components Constructed.") +time.sleep(5) + +engines = [] +for engine_port in robot_port_table.values(): + engines.append(Popen(["python3", "sub_route_guidance_engine.py", str(engine_port)])) +print("Sub Engine Constructed.") +time.sleep(5) + +toplevel_engine_p = Popen(["python3", "toplevel_route_guidance_engine.py"]) +print("Toplevel Engine Constructed.") +time.sleep(5) engine = Engine('http://127.0.0.1:8000') engine.connect() diff --git a/goal_sender/scripts/utilities.py b/goal_sender/scripts/utilities.py new file mode 100644 index 00000000..3bcaf8ff --- /dev/null +++ b/goal_sender/scripts/utilities.py @@ -0,0 +1,19 @@ +from subprocess import Popen + +class ComponentWrapper(object): + def __init__(self, component_name, robot_name, port): + command = ["rosrun", "goal_sender", component_name, str(port), robot_name] + self.process = Popen(command) + + def __del__(self): + self.process.kill() + + +class NavigationComponent(ComponentWrapper): + def __init__(self, robot_name, port): + super().__init__("Navigation.py", robot_name, port) + + +class SysInfoComponent(ComponentWrapper): + def __init__(self, robot_name, port): + super().__init__("System_Information.py", robot_name, port) -- GitLab From 523056956f77f030c8522fbd1223c897892e1c8f Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 17:27:35 +0900 Subject: [PATCH 57/72] Add VirtualNavigation component --- goal_sender/scripts/VirtualNavigation.py | 156 +++++++++++++++++++++++ 1 file changed, 156 insertions(+) create mode 100755 goal_sender/scripts/VirtualNavigation.py diff --git a/goal_sender/scripts/VirtualNavigation.py b/goal_sender/scripts/VirtualNavigation.py new file mode 100755 index 00000000..d7ba78f9 --- /dev/null +++ b/goal_sender/scripts/VirtualNavigation.py @@ -0,0 +1,156 @@ +#!/usr/bin/env python + +# VirtualNavigation.py +# +# Copyright 2018 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python3 +# For HRI Component + +import sys +# import queue +import time +from datetime import datetime +import threading + +from pyrois import RoIS_Common, RoIS_HRI + +if sys.version_info.major == 2: + import Queue as queue + import SocketServer + import SimpleXMLRPCServer + + class ThreadingXMLRPCServer(SocketServer.ThreadingMixIn, SimpleXMLRPCServer.SimpleXMLRPCServer): + pass +else: + import queue + import socketserver + import xmlrpc.server + + class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer): + pass + + +class Command(RoIS_Common.Command): + def __init__(self, c): + self._component = c + + def start(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def stop(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def suspend(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def resume(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + +class Query(RoIS_Common.Query): + def __init__(self, c): + self._component = c + + def component_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + c_status = RoIS_Common.Component_Status.UNINITIALIZED.value + return (status, c_status) + + +class Event(RoIS_Common.Event): + def __init__(self, c): + self._component = c + self.event_queue = queue.Queue() + + # def poll_event(self): + # msg = self.event_queue.get() + # return msg + +from goal_sender_ros import GoalSenderROS + +class VirtualNavigation(Event, Command, Query): + def __init__(self, c, robot_names): + super(VirtualNavigation, self).__init__(c) + self._component = c + self._component.Target_Position = [""] + self._component.Time_Limit = 10 + self._component.Routing_Policy = "" + + def set_parameter(self, target_position, time_limit, routing_policy): + self._component.send_goal_to_nearest_robot(target_position) + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def component_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + c_status = self._goal_sender.get_goalsender_state().value + return (status, c_status) + + +def event_dispatch(n): + # n.person_detected(datetime.now().isoformat(), 1) + time.sleep(0.1) + # n.person_detected(datetime.now().isoformat(), 1) + +import numpy as np +import rospy +from geometry_msgs.msg import PoseWithCovarianceStamped + +class Component(object): + def __init__(self, robot_names): + self.__robot_names = robot_names + self.__goal_senders = \ + {robot_name : GoalSenderROS(robot_name) for robot_name in robot_names} + self.__robot_positions = {robot_name : None for robot_name in robot_names} + for robot_name in robot_names: + rospy.Subscriber(robot_name+"/amcl_pose", + PoseWithCovarianceStamped, + lambda amcl_pose: self.update_position(robot_name, amcl_pose)) + + + def update_position(self, robot_name, amcl_pose): + pos = amcl_pose.pose.pose.position + self.__robot_positions[robot_name] = np.array([pos.x, pos.y]) + + + def send_goal_to_nearest_robot(self, dest): + dest = np.array(dest) + distance_table = \ + {name: np.linalg.norm(dest - pos) for name, pos in self.__robot_positions.items()} + nearest_robot = min(distance_table, key=distance_table.get) + self.__goal_senders[nearest_robot].send_goal(dest) + + +def example_n(port, robot_names): + print("Starting node...") + component = Component(robot_names) + n = VirtualNavigation(component, robot_names) + print("VirtualNavigation constructed") + + # start the timer to dispatch events + t = threading.Timer(0.1, event_dispatch, args=(n,)) + t.start() + + # start the XML-RPC server + server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) + server.register_instance(n) + server.register_introspection_functions() + server.register_multicall_functions() + server.serve_forever() + + +if __name__ == '__main__': + if len(sys.argv) < 3: + print("rosrun VirtualNavigation.py ...") + raise RuntimeError + + port, robot_names = int(sys.argv[1]), sys.argv[2:] + rospy.init_node('navigation_component', anonymous=True) + example_n(port=port, robot_names=robot_names) -- GitLab From 546ef463e86f26bbe5d80ec2e7f2cb8b9e9b588a Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 17:41:17 +0900 Subject: [PATCH 58/72] Add client of VirtualNavigation component --- .../scripts/VirtualNavigation_client.py | 107 ++++++++++++++++++ 1 file changed, 107 insertions(+) create mode 100644 goal_sender/scripts/VirtualNavigation_client.py diff --git a/goal_sender/scripts/VirtualNavigation_client.py b/goal_sender/scripts/VirtualNavigation_client.py new file mode 100644 index 00000000..be14ab33 --- /dev/null +++ b/goal_sender/scripts/VirtualNavigation_client.py @@ -0,0 +1,107 @@ +# VirtualNavigation_Client.py +# +# Copyright 2018 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python3 +# For HRI Engine + +"""VirtualNavigation_Client +""" + +import threading +# import logging +import xmlrpc.client + +from pyrois import RoIS_Common, RoIS_HRI + + +class Command(RoIS_Common.Command): + """Command + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def start(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.start()) + return status + + def stop(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.stop()) + return status + + def suspend(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.suspend()) + return status + + def resume(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.resume()) + return status + + def set_parameter(self, target_position, time_limit, routing_policy): + status = self._proxy.set_parameter(target_position, time_limit, routing_policy) + status = RoIS_HRI.ReturnCode_t(status) + return status + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def component_status(self): + status, c_status = self._proxy.component_status() + return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status)) + + def get_parameter(self): + (status, target_position, time_limit, routing_policy) = self._proxy.get_parameter() + return (RoIS_HRI.ReturnCode_t(status), target_position, time_limit, routing_policy) + + +class Event(RoIS_Common.Event): + """Event + """ + def __init__(self, uri): + self._uri = uri + self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + self.events = [] + # if logger is not None: + # self.logger = logger + + # def start_th(self): + # self.th = threading.Thread(target=self.event_loop) + # self.th.start() + + # def event_loop(self): + # """event_loop + # """ + # while True: + # try: + # self.poll_event() + # except ConnectionRefusedError: + # break + + # def poll_event(self): + # """poll_event + # """ + # msg = self._e_proxy.poll_event() + # (params, methodname) = xmlrpc.client.loads(msg) + # #self.logger.debug('poll_event: '+methodname) + # print(params,methodname) + # if methodname == 'speech_recognized': + # self.speech_recognized(params[0], params[1]) + + +class VirtualNavigation_Client(Command, Query, Event): + """VirtualNavigation_Client + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + self.events = [] + # self.start_th() -- GitLab From 062c880db38e5fee101668a884823ac8080e9d40 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 17:48:09 +0900 Subject: [PATCH 59/72] Add sample of set_parameter utilizing virtual component --- goal_sender/scripts/toplevel_route_guidance_engine.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/goal_sender/scripts/toplevel_route_guidance_engine.py b/goal_sender/scripts/toplevel_route_guidance_engine.py index aa5aa1e7..8dcb68c6 100644 --- a/goal_sender/scripts/toplevel_route_guidance_engine.py +++ b/goal_sender/scripts/toplevel_route_guidance_engine.py @@ -165,6 +165,7 @@ class EventIF(RoIS_HRI.EventIF): results = ["None"] return (status, results) +from VirtualNavigation_client import VirtualNavigation_Client as VNavClient from sub_route_guidance_engine_client import IF as EngineClient class IF(SystemIF, CommandIF, QueryIF, EventIF): @@ -173,6 +174,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): def __init__(self, Engine): super().__init__(Engine) self.command_id = 0 + self.component_clients = { + 'Navigation': VNavClient('http://localhost:8041') + } self.engine_clients = { 'robot1': EngineClient('http://localhost:8010'), 'robot2': EngineClient('http://localhost:8020'), @@ -186,7 +190,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): robot_distance_table = {} for name, engine in self.engine_clients.items(): pos = engine.get_position()[3] - print("OK1") pos = np.array(pos) robot_distance_table[name] = np.linalg.norm(dest - pos) nearest_robot = min(robot_distance_table, key=robot_distance_table.get) @@ -198,6 +201,11 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): self.command_id += 1 return (status, str(self.command_id)) + def set_parameter2(self, component_ref, parameters): + target_component = self.component_clients[component_ref] + status = target_component.set_parameter(*parameters).value + self.command_id += 1 + return (status, str(self.command_id)) class IF_server: """IF_Server -- GitLab From 9a17703bb972a8d2c3428dff09e46a18eb62a19e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 18:27:16 +0900 Subject: [PATCH 60/72] Fix VirtualNavigation component's set_parameter --- goal_sender/scripts/VirtualNavigation.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/goal_sender/scripts/VirtualNavigation.py b/goal_sender/scripts/VirtualNavigation.py index d7ba78f9..056888be 100755 --- a/goal_sender/scripts/VirtualNavigation.py +++ b/goal_sender/scripts/VirtualNavigation.py @@ -103,6 +103,8 @@ import numpy as np import rospy from geometry_msgs.msg import PoseWithCovarianceStamped +from functools import partial + class Component(object): def __init__(self, robot_names): self.__robot_names = robot_names @@ -111,8 +113,8 @@ class Component(object): self.__robot_positions = {robot_name : None for robot_name in robot_names} for robot_name in robot_names: rospy.Subscriber(robot_name+"/amcl_pose", - PoseWithCovarianceStamped, - lambda amcl_pose: self.update_position(robot_name, amcl_pose)) + PoseWithCovarianceStamped, + partial(self.update_position, robot_name)) def update_position(self, robot_name, amcl_pose): @@ -125,7 +127,8 @@ class Component(object): distance_table = \ {name: np.linalg.norm(dest - pos) for name, pos in self.__robot_positions.items()} nearest_robot = min(distance_table, key=distance_table.get) - self.__goal_senders[nearest_robot].send_goal(dest) + print("nearest_robot is ", nearest_robot) + self.__goal_senders[nearest_robot].send_goal(dest.tolist()) def example_n(port, robot_names): @@ -152,5 +155,5 @@ if __name__ == '__main__': raise RuntimeError port, robot_names = int(sys.argv[1]), sys.argv[2:] - rospy.init_node('navigation_component', anonymous=True) + rospy.init_node('virtual_navigation_component', anonymous=True) example_n(port=port, robot_names=robot_names) -- GitLab From 11e852afe8fbcc3e57209355f07a8ee34904258e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 18:28:52 +0900 Subject: [PATCH 61/72] Add debug code for virtual-component --- goal_sender/scripts/toplevel_route_guidance_engine.py | 1 + 1 file changed, 1 insertion(+) diff --git a/goal_sender/scripts/toplevel_route_guidance_engine.py b/goal_sender/scripts/toplevel_route_guidance_engine.py index 8dcb68c6..00464d04 100644 --- a/goal_sender/scripts/toplevel_route_guidance_engine.py +++ b/goal_sender/scripts/toplevel_route_guidance_engine.py @@ -202,6 +202,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): return (status, str(self.command_id)) def set_parameter2(self, component_ref, parameters): + print("Using VirtualNavigation") target_component = self.component_clients[component_ref] status = target_component.set_parameter(*parameters).value self.command_id += 1 -- GitLab From 6ea176fc2004d24ef5490fb2cd1dd7cad98e5ee8 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 18:29:25 +0900 Subject: [PATCH 62/72] Add client's function to call set_parameter2 --- .../scripts/toplevel_route_guidance_engine_client.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/goal_sender/scripts/toplevel_route_guidance_engine_client.py b/goal_sender/scripts/toplevel_route_guidance_engine_client.py index b522efb7..760af94a 100644 --- a/goal_sender/scripts/toplevel_route_guidance_engine_client.py +++ b/goal_sender/scripts/toplevel_route_guidance_engine_client.py @@ -73,6 +73,13 @@ class CommandIF(RoIS_HRI.CommandIF): status = RoIS_HRI.ReturnCode_t(s) return (status, command_id) + #### REMOVE #### + def set_parameter2(self, component_ref, parameters): + (s, command_id) = self._proxy.set_parameter2(component_ref, parameters) + status = RoIS_HRI.ReturnCode_t(s) + return (status, command_id) + ############### + def execute(self, command_unit_list): s = self._proxy.execute(command_unit_list) status = RoIS_HRI.ReturnCode_t(s) -- GitLab From aeee384cff57cdfb1a6ed74743348e886c29385f Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 18:31:27 +0900 Subject: [PATCH 63/72] Add test for VirtualNavigation component --- goal_sender/scripts/test_vnav_component.py | 33 ++++++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 goal_sender/scripts/test_vnav_component.py diff --git a/goal_sender/scripts/test_vnav_component.py b/goal_sender/scripts/test_vnav_component.py new file mode 100644 index 00000000..e0b9c89f --- /dev/null +++ b/goal_sender/scripts/test_vnav_component.py @@ -0,0 +1,33 @@ +# 1. launch navigation. +# 2. execute "rosrun goal_sender VirtualNavigation.py 8041 robot1 robot2 robot3". +# 3. run this test. + + +from toplevel_route_guidance_engine_client import IF as Engine +import time +from pyrois.RoIS_Common import Component_Status +from utilities import NavigationComponent, SysInfoComponent +from subprocess import Popen + +robot_port_table = { + 'robot1': 8010, 'robot2': 8020, 'robot3': 8030 +} +engines = [] +for engine_port in robot_port_table.values(): + engines.append(Popen(["python3", "sub_route_guidance_engine.py", str(engine_port)])) +print("Sub Engine Constructed.") +time.sleep(5) + +toplevel_engine_p = Popen(["python3", "toplevel_route_guidance_engine.py"]) +print("Toplevel Engine Constructed.") +time.sleep(5) + +engine = Engine('http://127.0.0.1:8000') +engine.connect() +for dest in [[25, 0], [0, 20], [5, 0]]: + status = engine.set_parameter2('Navigation', [dest, "", ""]) + print(status) + time.sleep(5) + +engine.disconnect() +print("finish") -- GitLab From 9ec55365fe50536d018854fd7b60c6cbe84e0a1d Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 20:48:59 +0900 Subject: [PATCH 64/72] Upgrade utilities function for test --- goal_sender/scripts/test_toplevel_engine.py | 21 ++---------- goal_sender/scripts/utilities.py | 37 +++++++++++++++++++++ 2 files changed, 39 insertions(+), 19 deletions(-) diff --git a/goal_sender/scripts/test_toplevel_engine.py b/goal_sender/scripts/test_toplevel_engine.py index a963bcf9..9a11ff57 100644 --- a/goal_sender/scripts/test_toplevel_engine.py +++ b/goal_sender/scripts/test_toplevel_engine.py @@ -8,26 +8,9 @@ from toplevel_route_guidance_engine_client import IF as Engine import time from pyrois.RoIS_Common import Component_Status -from utilities import NavigationComponent, SysInfoComponent -from subprocess import Popen +from utilities import setup_multi_robot -robot_port_table = {'robot1': 8010, 'robot2': 8020, 'robot3': 8030} -components = [] -for robot_name, engine_port in robot_port_table.items(): - components.append(NavigationComponent(robot_name, engine_port+1)) - components.append(SysInfoComponent(robot_name, engine_port+2)) -print("Components Constructed.") -time.sleep(5) - -engines = [] -for engine_port in robot_port_table.values(): - engines.append(Popen(["python3", "sub_route_guidance_engine.py", str(engine_port)])) -print("Sub Engine Constructed.") -time.sleep(5) - -toplevel_engine_p = Popen(["python3", "toplevel_route_guidance_engine.py"]) -print("Toplevel Engine Constructed.") -time.sleep(5) +process = setup_multi_robot() engine = Engine('http://127.0.0.1:8000') engine.connect() diff --git a/goal_sender/scripts/utilities.py b/goal_sender/scripts/utilities.py index 3bcaf8ff..e1954afa 100644 --- a/goal_sender/scripts/utilities.py +++ b/goal_sender/scripts/utilities.py @@ -1,3 +1,4 @@ +import time from subprocess import Popen class ComponentWrapper(object): @@ -17,3 +18,39 @@ class NavigationComponent(ComponentWrapper): class SysInfoComponent(ComponentWrapper): def __init__(self, robot_name, port): super().__init__("System_Information.py", robot_name, port) + + +class SubEngineWrapper(object): + def __init__(self, engine_port): + self.process = Popen(["python3", "sub_route_guidance_engine.py", str(engine_port)]) + + def __del__(self): + self.process.kill() + + +class ToplevelEngineWrapper(object): + def __init__(self): + self.process = Popen(["python3", "toplevel_route_guidance_engine.py"]) + + def __del__(self): + self.process.kill() + + +def setup_multi_robot(): + robot_port_table = {'robot1': 8010, 'robot2': 8020, 'robot3': 8030} + process = [] + for robot_name, engine_port in robot_port_table.items(): + process.append(NavigationComponent(robot_name, engine_port+1)) + process.append(SysInfoComponent(robot_name, engine_port+2)) + print("Components Constructed.") + time.sleep(5) + + for engine_port in robot_port_table.values(): + process.append(SubEngineWrapper(engine_port)) + print("Sub Engine Constructed.") + time.sleep(5) + + process.append(ToplevelEngineWrapper()) + print("Toplevel Engine Constructed.") + time.sleep(5) + return process -- GitLab From 0a6e6d691764375947e5447b2bb80d4a56ba3700 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 20:51:26 +0900 Subject: [PATCH 65/72] Add function to setup single robot system --- goal_sender/scripts/utilities.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/goal_sender/scripts/utilities.py b/goal_sender/scripts/utilities.py index e1954afa..76153773 100644 --- a/goal_sender/scripts/utilities.py +++ b/goal_sender/scripts/utilities.py @@ -36,6 +36,22 @@ class ToplevelEngineWrapper(object): self.process.kill() +def setup_single_robot(): + robot_port_table = {'robot1': 8010} + process = [] + for robot_name, engine_port in robot_port_table.items(): + process.append(NavigationComponent(robot_name, engine_port+1)) + process.append(SysInfoComponent(robot_name, engine_port+2)) + print("Components Constructed.") + time.sleep(5) + + for engine_port in robot_port_table.values(): + process.append(SubEngineWrapper(engine_port)) + print("Sub Engine Constructed.") + time.sleep(5) + + return process + def setup_multi_robot(): robot_port_table = {'robot1': 8010, 'robot2': 8020, 'robot3': 8030} process = [] -- GitLab From d7fe3ad8c180b4a542c5f45a47fd488ac208a427 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 23:35:45 +0900 Subject: [PATCH 66/72] Use robot setup function --- goal_sender/scripts/test_nav_component.py | 3 +++ goal_sender/scripts/test_sub_engine.py | 3 +++ goal_sender/scripts/test_sys_component.py | 6 +++++- goal_sender/scripts/test_vnav_component.py | 15 ++------------- 4 files changed, 13 insertions(+), 14 deletions(-) diff --git a/goal_sender/scripts/test_nav_component.py b/goal_sender/scripts/test_nav_component.py index 66fb8949..b0171018 100644 --- a/goal_sender/scripts/test_nav_component.py +++ b/goal_sender/scripts/test_nav_component.py @@ -4,6 +4,9 @@ import time from Navigation_client import Navigation_Client +from utilities import setup_single_robot + +process = setup_single_robot() uri = "http://localhost:8011" client = Navigation_Client(uri) diff --git a/goal_sender/scripts/test_sub_engine.py b/goal_sender/scripts/test_sub_engine.py index 7932218e..978537c0 100644 --- a/goal_sender/scripts/test_sub_engine.py +++ b/goal_sender/scripts/test_sub_engine.py @@ -7,6 +7,9 @@ from sub_route_guidance_engine_client import IF as Engine import time from pyrois.RoIS_Common import Component_Status +from utilities import setup_single_robot + +process = setup_single_robot() engine = Engine('http://127.0.0.1:8010') engine.connect() diff --git a/goal_sender/scripts/test_sys_component.py b/goal_sender/scripts/test_sys_component.py index 223bdb63..b002ff8d 100644 --- a/goal_sender/scripts/test_sys_component.py +++ b/goal_sender/scripts/test_sys_component.py @@ -4,10 +4,14 @@ import time from System_Information_client import System_Information_Client +from utilities import setup_single_robot + +process = setup_single_robot() uri = "http://localhost:8012" client = System_Information_Client(uri) -while True: +start_time = time.time() +while time.time() - start_time < 10: time.sleep(1) print(client.robot_position()) diff --git a/goal_sender/scripts/test_vnav_component.py b/goal_sender/scripts/test_vnav_component.py index e0b9c89f..3d5fdcd1 100644 --- a/goal_sender/scripts/test_vnav_component.py +++ b/goal_sender/scripts/test_vnav_component.py @@ -6,21 +6,10 @@ from toplevel_route_guidance_engine_client import IF as Engine import time from pyrois.RoIS_Common import Component_Status -from utilities import NavigationComponent, SysInfoComponent +from utilities import setup_multi_robot from subprocess import Popen -robot_port_table = { - 'robot1': 8010, 'robot2': 8020, 'robot3': 8030 -} -engines = [] -for engine_port in robot_port_table.values(): - engines.append(Popen(["python3", "sub_route_guidance_engine.py", str(engine_port)])) -print("Sub Engine Constructed.") -time.sleep(5) - -toplevel_engine_p = Popen(["python3", "toplevel_route_guidance_engine.py"]) -print("Toplevel Engine Constructed.") -time.sleep(5) +process = setup_multi_robot() engine = Engine('http://127.0.0.1:8000') engine.connect() -- GitLab From 2191b1807a47338ad93784943afeb652e4730f47 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 23:45:05 +0900 Subject: [PATCH 67/72] Refactor with SubprocessWrapper class --- goal_sender/scripts/utilities.py | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/goal_sender/scripts/utilities.py b/goal_sender/scripts/utilities.py index 76153773..5d9966f9 100644 --- a/goal_sender/scripts/utilities.py +++ b/goal_sender/scripts/utilities.py @@ -1,15 +1,20 @@ import time from subprocess import Popen -class ComponentWrapper(object): - def __init__(self, component_name, robot_name, port): - command = ["rosrun", "goal_sender", component_name, str(port), robot_name] + +class SubprocessWrapper(object): + def __init__(self, command): self.process = Popen(command) def __del__(self): self.process.kill() +class ComponentWrapper(SubprocessWrapper): + def __init__(self, component_name, robot_name, port): + super().__init__(["rosrun", "goal_sender", component_name, str(port), robot_name]) + + class NavigationComponent(ComponentWrapper): def __init__(self, robot_name, port): super().__init__("Navigation.py", robot_name, port) @@ -20,20 +25,14 @@ class SysInfoComponent(ComponentWrapper): super().__init__("System_Information.py", robot_name, port) -class SubEngineWrapper(object): +class SubEngineWrapper(SubprocessWrapper): def __init__(self, engine_port): - self.process = Popen(["python3", "sub_route_guidance_engine.py", str(engine_port)]) - - def __del__(self): - self.process.kill() + super().__init__(["python3", "sub_route_guidance_engine.py", str(engine_port)]) -class ToplevelEngineWrapper(object): +class ToplevelEngineWrapper(SubprocessWrapper): def __init__(self): - self.process = Popen(["python3", "toplevel_route_guidance_engine.py"]) - - def __del__(self): - self.process.kill() + super().__init__(["python3", "toplevel_route_guidance_engine.py"]) def setup_single_robot(): -- GitLab From 0a6c9750b341cd45bc143857e14b93caba3595d4 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 6 Nov 2019 00:05:09 +0900 Subject: [PATCH 68/72] Create process of VirtualNavigation on test_vnav_component --- goal_sender/scripts/test_vnav_component.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/goal_sender/scripts/test_vnav_component.py b/goal_sender/scripts/test_vnav_component.py index 3d5fdcd1..74f71edf 100644 --- a/goal_sender/scripts/test_vnav_component.py +++ b/goal_sender/scripts/test_vnav_component.py @@ -6,10 +6,15 @@ from toplevel_route_guidance_engine_client import IF as Engine import time from pyrois.RoIS_Common import Component_Status -from utilities import setup_multi_robot -from subprocess import Popen +from utilities import setup_multi_robot, SubprocessWrapper process = setup_multi_robot() +command = [ + "rosrun", "goal_sender", "VirtualNavigation.py", + "8041", "robot1", "robot2", "robot3" +] +process.append(SubprocessWrapper(command)) +time.sleep(5) engine = Engine('http://127.0.0.1:8000') engine.connect() -- GitLab From 730b89e14e44097815dc1bfe4d69659e15d57933 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 6 Nov 2019 00:47:07 +0900 Subject: [PATCH 69/72] Refactor functions to spawn rois system --- goal_sender/scripts/utilities.py | 22 ++++++++-------------- 1 file changed, 8 insertions(+), 14 deletions(-) diff --git a/goal_sender/scripts/utilities.py b/goal_sender/scripts/utilities.py index 5d9966f9..9fa84a5f 100644 --- a/goal_sender/scripts/utilities.py +++ b/goal_sender/scripts/utilities.py @@ -35,8 +35,7 @@ class ToplevelEngineWrapper(SubprocessWrapper): super().__init__(["python3", "toplevel_route_guidance_engine.py"]) -def setup_single_robot(): - robot_port_table = {'robot1': 8010} +def launch_components_and_subengines(**robot_port_table): process = [] for robot_name, engine_port in robot_port_table.items(): process.append(NavigationComponent(robot_name, engine_port+1)) @@ -51,21 +50,16 @@ def setup_single_robot(): return process -def setup_multi_robot(): - robot_port_table = {'robot1': 8010, 'robot2': 8020, 'robot3': 8030} - process = [] - for robot_name, engine_port in robot_port_table.items(): - process.append(NavigationComponent(robot_name, engine_port+1)) - process.append(SysInfoComponent(robot_name, engine_port+2)) - print("Components Constructed.") - time.sleep(5) - for engine_port in robot_port_table.values(): - process.append(SubEngineWrapper(engine_port)) - print("Sub Engine Constructed.") - time.sleep(5) +def setup_single_robot(): + return launch_components_and_subengines(robot1=8010) + + +def setup_multi_robot(): + process = launch_components_and_subengines(robot1=8010, robot2=8020, robot3=8030) process.append(ToplevelEngineWrapper()) print("Toplevel Engine Constructed.") time.sleep(5) + return process -- GitLab From db14f4a16e5ff2cc3b847539f54c8ae198a21fec Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 6 Nov 2019 00:58:11 +0900 Subject: [PATCH 70/72] Upgrade comments on test scripts --- goal_sender/scripts/test_nav_component.py | 6 +++--- goal_sender/scripts/test_sub_engine.py | 6 ++---- goal_sender/scripts/test_sys_component.py | 6 +++--- goal_sender/scripts/test_toplevel_engine.py | 7 ++----- goal_sender/scripts/test_vnav_component.py | 5 ++--- 5 files changed, 12 insertions(+), 18 deletions(-) diff --git a/goal_sender/scripts/test_nav_component.py b/goal_sender/scripts/test_nav_component.py index b0171018..8724364d 100644 --- a/goal_sender/scripts/test_nav_component.py +++ b/goal_sender/scripts/test_nav_component.py @@ -1,6 +1,6 @@ -# 1. launch navigation. -# 2. execute "rosrun (this_package) Navigation.py 8011 robot1" -# 3. run this test +# 1. launch single navigation. +# 2. run this test. + import time from Navigation_client import Navigation_Client diff --git a/goal_sender/scripts/test_sub_engine.py b/goal_sender/scripts/test_sub_engine.py index 978537c0..a237dd07 100644 --- a/goal_sender/scripts/test_sub_engine.py +++ b/goal_sender/scripts/test_sub_engine.py @@ -1,7 +1,5 @@ -# 1. launch navigation. -# 2. execute "rosrun (this_package) Navigation.py 8001 robot1". -# 3. run sub_route_guidance_engine server with server and component port. -# 4. run this test. +# 1. launch single navigation. +# 2. run this test. from sub_route_guidance_engine_client import IF as Engine diff --git a/goal_sender/scripts/test_sys_component.py b/goal_sender/scripts/test_sys_component.py index b002ff8d..556e5ff7 100644 --- a/goal_sender/scripts/test_sys_component.py +++ b/goal_sender/scripts/test_sys_component.py @@ -1,6 +1,6 @@ -# 1. launch navigation. -# 2. execute "rosrun (this_package) System_Information.py 8012 robot1" -# 3. run this test +# 1. launch single navigation. +# 2. run this test. + import time from System_Information_client import System_Information_Client diff --git a/goal_sender/scripts/test_toplevel_engine.py b/goal_sender/scripts/test_toplevel_engine.py index 9a11ff57..0cc1fe2b 100644 --- a/goal_sender/scripts/test_toplevel_engine.py +++ b/goal_sender/scripts/test_toplevel_engine.py @@ -1,8 +1,5 @@ -# 1. launch navigation. -# 2. execute "rosrun (this_package) Navigation.py". -# 3. run sub_route_guidance_engine server. -# 4. run toplevel_route_guidance_engine server. -# 5. run this test. +# 1. launch multi navigation. +# 2. run this test. from toplevel_route_guidance_engine_client import IF as Engine diff --git a/goal_sender/scripts/test_vnav_component.py b/goal_sender/scripts/test_vnav_component.py index 74f71edf..23947c33 100644 --- a/goal_sender/scripts/test_vnav_component.py +++ b/goal_sender/scripts/test_vnav_component.py @@ -1,6 +1,5 @@ -# 1. launch navigation. -# 2. execute "rosrun goal_sender VirtualNavigation.py 8041 robot1 robot2 robot3". -# 3. run this test. +# 1. launch multi navigation. +# 2. run this test. from toplevel_route_guidance_engine_client import IF as Engine -- GitLab From 22c9de0df2fe31fc706e29b0b11a01cd4458dd52 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 6 Nov 2019 15:04:04 +0900 Subject: [PATCH 71/72] Remove unnecessary package (connection_test) --- connection_test/CMakeLists.txt | 50 ---- connection_test/package.xml | 21 -- connection_test/scripts/.gitignore | 2 - .../scripts/Person_Detection_client.py | 109 --------- connection_test/scripts/README.md | 14 -- connection_test/scripts/listener.py | 60 ----- connection_test/scripts/pyrois/.gitignore | 1 - .../scripts/pyrois/HRI_Engine_client.py | 129 ---------- .../scripts/pyrois/HRI_Engine_example.py | 205 ---------------- .../scripts/pyrois/Person_Detection.py | 134 ---------- .../scripts/pyrois/Person_Detection_client.py | 104 -------- connection_test/scripts/pyrois/RoIS_Common.py | 67 ----- .../scripts/pyrois/RoIS_Common.pyc | Bin 2779 -> 0 bytes connection_test/scripts/pyrois/RoIS_HRI.py | 142 ----------- connection_test/scripts/pyrois/RoIS_HRI.pyc | Bin 6257 -> 0 bytes .../scripts/pyrois/RoIS_Service.py | 55 ----- .../Service_Application_Base_example.py | 100 -------- .../scripts/pyrois/Service_Application_IF.py | 81 ------ connection_test/scripts/pyrois/__init__.py | 0 connection_test/scripts/pyrois/__init__.pyc | Bin 164 -> 0 bytes connection_test/scripts/pyrois/unittest.py | 231 ------------------ connection_test/scripts/talker.py | 138 ----------- connection_test/scripts/test.py | 11 - 23 files changed, 1654 deletions(-) delete mode 100644 connection_test/CMakeLists.txt delete mode 100644 connection_test/package.xml delete mode 100644 connection_test/scripts/.gitignore delete mode 100755 connection_test/scripts/Person_Detection_client.py delete mode 100644 connection_test/scripts/README.md delete mode 100755 connection_test/scripts/listener.py delete mode 100644 connection_test/scripts/pyrois/.gitignore delete mode 100644 connection_test/scripts/pyrois/HRI_Engine_client.py delete mode 100644 connection_test/scripts/pyrois/HRI_Engine_example.py delete mode 100644 connection_test/scripts/pyrois/Person_Detection.py delete mode 100644 connection_test/scripts/pyrois/Person_Detection_client.py delete mode 100644 connection_test/scripts/pyrois/RoIS_Common.py delete mode 100644 connection_test/scripts/pyrois/RoIS_Common.pyc delete mode 100644 connection_test/scripts/pyrois/RoIS_HRI.py delete mode 100644 connection_test/scripts/pyrois/RoIS_HRI.pyc delete mode 100644 connection_test/scripts/pyrois/RoIS_Service.py delete mode 100644 connection_test/scripts/pyrois/Service_Application_Base_example.py delete mode 100644 connection_test/scripts/pyrois/Service_Application_IF.py delete mode 100644 connection_test/scripts/pyrois/__init__.py delete mode 100644 connection_test/scripts/pyrois/__init__.pyc delete mode 100644 connection_test/scripts/pyrois/unittest.py delete mode 100755 connection_test/scripts/talker.py delete mode 100644 connection_test/scripts/test.py diff --git a/connection_test/CMakeLists.txt b/connection_test/CMakeLists.txt deleted file mode 100644 index a2eaf3cc..00000000 --- a/connection_test/CMakeLists.txt +++ /dev/null @@ -1,50 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(connection_test) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - rospy - std_msgs -) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# CATKIN_DEPENDS rospy std_msgs -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_connection_test.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/connection_test/package.xml b/connection_test/package.xml deleted file mode 100644 index 21c8e6fa..00000000 --- a/connection_test/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - connection_test - 0.0.0 - The connection_test package - - tanacchi - - MIT - - catkin - rospy - std_msgs - rospy - std_msgs - rospy - std_msgs - - - - diff --git a/connection_test/scripts/.gitignore b/connection_test/scripts/.gitignore deleted file mode 100644 index 8d35cb32..00000000 --- a/connection_test/scripts/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -__pycache__ -*.pyc diff --git a/connection_test/scripts/Person_Detection_client.py b/connection_test/scripts/Person_Detection_client.py deleted file mode 100755 index 5fc9af96..00000000 --- a/connection_test/scripts/Person_Detection_client.py +++ /dev/null @@ -1,109 +0,0 @@ -# Person_Detection_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 - -"""Person_Detection_Client -""" - -import sys -import threading -# import logging - -if sys.version_info.major == 2: - import xmlrpclib as xmlrpc_client -else: - import xmlrpc.client as 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 Person_Detection_Client(Command, Query, Event): - """Person_Detection_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() diff --git a/connection_test/scripts/README.md b/connection_test/scripts/README.md deleted file mode 100644 index 6e78e79c..00000000 --- a/connection_test/scripts/README.md +++ /dev/null @@ -1,14 +0,0 @@ -# connection_test - -RoISとROSの接続テスト - -ROSノードをコンポーネントとする -エンジンを作成し、クライアントからのリクエストでトピックを送信する。 - -# How to use -1. roscore を起動 -2. 別ターミナルで`rosrun connection_test talker.py`を実行 -3. さらに別のターミナルで `python test.py` を実行 - -Already Used 的なエラーが出たらポートの番号を変えてみること。 -SIGINT でキルできなかったらターミナルごと閉じること。 diff --git a/connection_test/scripts/listener.py b/connection_test/scripts/listener.py deleted file mode 100755 index ed27e1e5..00000000 --- a/connection_test/scripts/listener.py +++ /dev/null @@ -1,60 +0,0 @@ -#!/usr/bin/env python -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Revision $Id$ - -## Simple talker demo that listens to std_msgs/Strings published -## to the 'chatter' topic - -import rospy -from std_msgs.msg import String - -def callback(data): - rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data.data) - -def listener(): - - # In ROS, nodes are uniquely named. If two nodes with the same - # name are launched, the previous one is kicked off. The - # anonymous=True flag means that rospy will choose a unique - # name for our 'listener' node so that multiple listeners can - # run simultaneously. - rospy.init_node('listener', anonymous=True) - - rospy.Subscriber('chatter', String, callback) - - # spin() simply keeps python from exiting until this node is stopped - rospy.spin() - -if __name__ == '__main__': - listener() diff --git a/connection_test/scripts/pyrois/.gitignore b/connection_test/scripts/pyrois/.gitignore deleted file mode 100644 index bee8a64b..00000000 --- a/connection_test/scripts/pyrois/.gitignore +++ /dev/null @@ -1 +0,0 @@ -__pycache__ diff --git a/connection_test/scripts/pyrois/HRI_Engine_client.py b/connection_test/scripts/pyrois/HRI_Engine_client.py deleted file mode 100644 index 6c79f528..00000000 --- a/connection_test/scripts/pyrois/HRI_Engine_client.py +++ /dev/null @@ -1,129 +0,0 @@ -# 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 xmlrpc.client - -from pyrois import RoIS_HRI - - -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): - """IF - """ - def __init__(self, uri): - self._uri = uri - self._proxy = xmlrpc.client.ServerProxy(self._uri) - -if __name__ == "__main__": - pass diff --git a/connection_test/scripts/pyrois/HRI_Engine_example.py b/connection_test/scripts/pyrois/HRI_Engine_example.py deleted file mode 100644 index 7b6770a6..00000000 --- a/connection_test/scripts/pyrois/HRI_Engine_example.py +++ /dev/null @@ -1,205 +0,0 @@ -# HRI_Engine_example.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 - -"""HRI_Engine_example -""" - -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 MyHRIE: - """IF_Server - """ - def __init__(self): - self.state = False - - -def test_engine(port): - """test_engine - """ - IF_server(port).run(IF(MyHRIE())) - - -if __name__ == "__main__": - pass diff --git a/connection_test/scripts/pyrois/Person_Detection.py b/connection_test/scripts/pyrois/Person_Detection.py deleted file mode 100644 index 790033af..00000000 --- a/connection_test/scripts/pyrois/Person_Detection.py +++ /dev/null @@ -1,134 +0,0 @@ -# Person_Detection.py -# -# Copyright 2018 Eiichi Inohira -# This software may be modified and distributed under the terms -# of the MIT license -# -# For python3 -# For HRI Component - -"""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 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 - - def start(self): - status = RoIS_HRI.ReturnCode_t.OK.value - return status - - def stop(self): - status = RoIS_HRI.ReturnCode_t.OK.value - return status - - def suspend(self): - status = RoIS_HRI.ReturnCode_t.OK.value - return status - - def resume(self): - status = RoIS_HRI.ReturnCode_t.OK.value - return status - - -class Query(RoIS_Common.Query): - """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): - """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_detected(self, timestamp, number): - """person_detected - """ - msg = xmlrpc.client.dumps((timestamp, number), 'person_detected') - self.event_queue.put(msg) - - -class Person_Detection(Event, Command, Query): - """Person_Detection - """ - pass - - -class component: - """component - """ - def __init__(self): - self._state = False - - -def event_dispatch(pd): - """event_dispatch - """ - pd.person_detected(datetime.now().isoformat(), 1) - time.sleep(0.1) - pd.person_detected(datetime.now().isoformat(), 1) - - -def example_pd(port): - """example_pd - """ - c = component() - pd = Person_Detection(c) - - # start the timer to dispatch events - t = threading.Timer(0.1, event_dispatch, args=(pd,)) - t.start() - - # start the XML-RPC server - server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) - server.register_instance(pd) - server.register_introspection_functions() - server.register_multicall_functions() - server.serve_forever() - - -if __name__ == '__main__': - example_pd(8000) diff --git a/connection_test/scripts/pyrois/Person_Detection_client.py b/connection_test/scripts/pyrois/Person_Detection_client.py deleted file mode 100644 index 77f4af4b..00000000 --- a/connection_test/scripts/pyrois/Person_Detection_client.py +++ /dev/null @@ -1,104 +0,0 @@ -# Person_Detection_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 - -"""Person_Detection_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 Person_Detection_Client(Command, Query, Event): - """Person_Detection_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() diff --git a/connection_test/scripts/pyrois/RoIS_Common.py b/connection_test/scripts/pyrois/RoIS_Common.py deleted file mode 100644 index 7cacd5aa..00000000 --- a/connection_test/scripts/pyrois/RoIS_Common.py +++ /dev/null @@ -1,67 +0,0 @@ -# RoIS_Common.py -# -# Copyright 2017 Eiichi Inohira -# This software may be modified and distributed under the terms -# of the MIT license -# -# For python3 -# For HRI Component -""" -RoIS_Common -""" -import abc -import enum - -class Component_Status(enum.Enum): - """Component_Status - """ - UNINITIALIZED = 0 - READY = 1 - BUSY = 2 - WARNING = 3 - ERROR = 4 - -class Command: - """Command - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def start(self): - """ - start () -> status: RoIS_HRI.ReturnCode_t - """ - pass - @abc.abstractmethod - def stop(self): - """ - stop () -> status: RoIS_HRI.ReturnCode_t - """ - pass - @abc.abstractmethod - def suspend(self): - """ - suspend () -> status: RoIS_HRI.ReturnCode_t - """ - pass - @abc.abstractmethod - def resume(self): - """ - resume () -> status: RoIS_HRI.ReturnCode_t - """ - pass - -class Query: - """Quary - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def component_status(self): - """ - component_status () -> (status: RoIS_HRI.ReturnCode_t, status: Component_Status) - """ - pass - -class Event: - """Event - """ - pass diff --git a/connection_test/scripts/pyrois/RoIS_Common.pyc b/connection_test/scripts/pyrois/RoIS_Common.pyc deleted file mode 100644 index 4c139e1a0bb898757ba6e3007e3b8d2c07dac4ef..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2779 zcmd5;TTc@~6h6D9g_hz4G|@y8;?t5S`{aX&1PYBd(JE7kp^4dScLrIcyUos2QeX9_ z_ybM+0eZe!y40E|F@dnnoSr#zJJ)Zf^kbp+edF8vh)%x}|8H{BhKVHVGIdD0bnH-< znVfg2>yj*ya_Oi<2P~YSuS8#n_PaAA%OvMXRtgdKXGzvc&K05x$tuYOlC?s_Q(WqJ z*Kqm*lbT3donSld_tT^n|KzW2Zm^ssgEUcz2|6Y;LtW_Z@L&V0A2Ly_JL|=iHD%pP z*d5B1sLT4yP?uFGQ+Jj`$E#q~Z6?!u*OvKR6Je6R)ZvJn)lM>r@LHl?VYDw{6UT6ZyQ32j%orCUzo1s7;ue zWp8Eq(FSS!+PS}RfQ^n;#|IZ20vm9v={*<#E2#W0 zGuL=hp|m#X;D4KqPN&^!1A`2s1jqO*8>kn8M1bt?s<4|ixN5FpA(m_J*BG zD1lo_V4=7PvIugE$)zO65+C%Kn??cPnyfncMg56iBt=@F1R9uyq@7F)jy=X~?3-9( zJ@27Lrp&w)Yw+gTggyXfSTl8M?tiNtyl!tol??lqX))u}sm_ilCR{?hy}ZAfr4 diff --git a/connection_test/scripts/pyrois/RoIS_HRI.py b/connection_test/scripts/pyrois/RoIS_HRI.py deleted file mode 100644 index 4f3b7536..00000000 --- a/connection_test/scripts/pyrois/RoIS_HRI.py +++ /dev/null @@ -1,142 +0,0 @@ -# RoIS_HRI.py -# -# Copyright 2017 Eiichi Inohira -# This software may be modified and distributed under the terms -# of the MIT license -# -# for python 3 - -"""RoIS_HRI -""" -import abc -import enum - -class ReturnCode_t(enum.Enum): - """ReturnCode_t - """ - OK = 1 - ERROR = 2 - BAD_PARAMETER = 3 - UNSUPPORTED = 4 - OUT_OF_RESOURCES = 5 - TIMEOUT = 6 - -class SystemIF: - """ - class SystemIF(object) - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def connect(self): - """ - connect() -> ReturnCode_t: status - """ - pass - @abc.abstractmethod - def disconnect(self): - """ - disconnect() -> ReturnCode_t: status - """ - pass - @abc.abstractmethod - def get_profile(self, condition): - """ - get_profile(condition) -> (status, profile) - """ - pass - @abc.abstractmethod - def get_error_detail(self, error_id, condition): - """ - get_error_detail(error_id,condition) -> (status,results) - """ - pass - -class CommandIF: - """ - class CommandIF(object): - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def search(self, condition): - """ - search(condition) -> (status, component_ref_list) - """ - pass - @abc.abstractmethod - def bind(self, component_ref): - """ - bind(component_ref) -> status - """ - pass - @abc.abstractmethod - def bind_any(self, condition): - """ - bind_any(condition) -> (status,component_ref_list) - """ - pass - @abc.abstractmethod - def release(self, component_ref): - """ - release(component_ref) -> status - """ - pass - @abc.abstractmethod - def get_parameter(self, component_ref): - """ - get_parameter(self,component_ref) -> (status,parameter_list) - """ - pass - @abc.abstractmethod - def set_parameter(self, component_ref, parameters): - """ - set_parameter(self, component_ref, parameters) -> (status,command_id) - """ - pass - @abc.abstractmethod - def execute(self, command_unit_list): - """ - execute(command_unit_list) -> status - """ - pass - @abc.abstractmethod - def get_command_result(self, command_id, condition): - """ - get_command_result(self, command_id, condition) -> (status,results) - """ - pass - -class QueryIF: - """ - class QueryIF(object): - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def query(self, query_type, condition): - """ - query(self, query_type, condition) -> (status,results) - """ - pass - -class EventIF: - """ - class EventIF(object): - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def subscribe(self, event_type, condition): - """ - subscribe(self, event_type, condition) -> (status,subscribe_id) - """ - pass - @abc.abstractmethod - def unsubscribe(self, subscribe_id): - """ - unsubscribe(self,subscribe_id) -> status - """ - pass - @abc.abstractmethod - def get_event_detail(self, event_id, condition): - """ - get_event_detail(self,event_id,condition) -> (status,results) - """ - pass diff --git a/connection_test/scripts/pyrois/RoIS_HRI.pyc b/connection_test/scripts/pyrois/RoIS_HRI.pyc deleted file mode 100644 index 59dd27fdc82a83272b2284ff63f5763aa479cebc..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6257 zcmc&&ZEqVz5MJAf5a9`_8>x z(*}u8MMC@!B>o-}KLF<0y*-~@J9cRf9NlDZ_jY%lnP+F_uJYG<{m)y!fA6#OSE2u3 z(ItN+5ir&vSz*01s7ScmUY#zbK7|h3aygh@lM;l$9L-C-}Kv4Yv>bd zm_*xgAY7HfDAB-e%=(msAQ+?~Nm80%RIn=atgsHLw!}K5%`)qd0#(+jG2V7oFc;hA zSFbnd7h>tK3;|n(OAyNtRT2)gR5cQ=8+m=y_#wz>YD$H(i zzR%Sq_gbGX+&ynlb6AGGeMily9QQ?2d6DP)$6@nnocti0kG!8mH+G-OrcC^%A4ie!RTxLE5>hp# zpM(P?n}d@i4rSBY{gc57#|G!=k`UE$wKV$Ez}qKMiT=*r%ojkZec@6g?+a03kIDGU z3=MKYN&B4~v|@Y8wx?!$Dz<0U_88{TtU4SATgnCr?JU3NN!hR(IdS(ffxK>p;75CH zV-`F~Idved%9t$fhC`Iq6NOx%8r|P|r z{h&Ns_AN5}pMvfNb!R#Uq5M|>ImlTkIGml4cGu|6;sZ=vGw^*$Lm9qDLb-z^J_>un zA)rABo@-Y+#(SHMbl6Y>9#8#^bHp3^p?uV>U>H9QKra!35aPn4m^2y9meWFVqZ(bTC66LzDVaTR1QihatFj z64X=>Uf0)$V(a$yE&!orQYD_RsQ)+)xK1R9zWf3NBL4`SZWRzo z!S1)|lGwdksg_ntm#X;FWZRDWeJ={81=2t0`9cF}8jzQ5usvskt;y*WY}ahhihX9? z_LwA3&zwg<#<`JKNoZo4fSf_I5&5PWgd=gyp!c?21qW0LFY%9Ooo7Gp58_Bf%1y+P z+Y4nH^`{-};;D?v!07uqT7XC)dJbA}Jv+?ZFbW834wOE-(~}PtHYXO09-B2H_1W`T z=Q}96MY}Et^3Ehk*NaYOICTuj|L)ToX%4VZ`0K?1qaMw?nuwn8B$CogI+i1B6zWTq zsi}G>As2M#ULKv!VO~O2uo8)bLWnZYBv6}y2ll~vjx8KH&diwV?Hqn27+fi_s#5G6HrPgY*y`i^a0Ba#K7L z{!nSwYm~z%RL1M5uIGg`q-JZMY-XXraTHRW@djfd&!NI0J(^Jge6mWLI%~v-)YF^9 zjcKLn{IU#Xgk3xgiqB@3XulT$Q9WW!G*4bw+{wD1Lj!Nj3uf$9}Mim)I@L(vClWFxBO7sQmF{+4gWF}ux)1@cs zp*a-0M+;>XdjjQBR_V5@P6p!DST$Owcvj)XUGMh<<}qQ#sZo2P#-B>059qn#S#phb z=|MZxbA~0F7p)(u(458#WX=V7kyhNNe()gf=uZj_T2}krRMYA9FUOq+b`4!i54-r_ z(-kRy1OT1NA8BWtnxk}j8ZA~liS)~cRqbbjXoEHQglk6LzL~eiFv`#wdt|o&yGV}0 z`&@p~h?-{~yYv^roXXF!w>dukocOVyM~#SXT$O$<&;^_6dDU+Qnoj>kYRbMD=p7A^ z-$n*;kSZ66CvX`52x61O$bGtX_CDkv(k{>Fl2CAze}YUS$?*g zcl|n3_-WyJaO=v^h0s)UbE+KS4|? Date: Wed, 6 Nov 2019 15:40:58 +0900 Subject: [PATCH 72/72] Rename package (goal_sedner => route_guidance_ros) --- goal_sender/CMakeLists.txt | 206 ------------------ goal_sender/package.xml | 68 ------ route_guidance_ros/CMakeLists.txt | 21 ++ route_guidance_ros/package.xml | 26 +++ .../scripts/Navigation.py | 0 .../scripts/Navigation_client.py | 0 .../scripts/System_Information.py | 0 .../scripts/System_Information_client.py | 0 .../scripts/VirtualNavigation.py | 0 .../scripts/VirtualNavigation_client.py | 0 .../scripts/goal_sender_ros.py | 0 .../scripts/sub_engine.py | 6 +- .../scripts/sub_engine_client.py | 0 .../scripts/test_nav_component.py | 0 .../scripts/test_sub_engine.py | 2 +- .../scripts/test_sys_component.py | 0 .../scripts/test_toplevel_engine.py | 2 +- .../scripts/test_vnav_component.py | 6 +- .../scripts/toplevel_engine.py | 2 +- .../scripts/toplevel_engine_client.py | 0 .../scripts/utilities.py | 6 +- 21 files changed, 59 insertions(+), 286 deletions(-) delete mode 100644 goal_sender/CMakeLists.txt delete mode 100644 goal_sender/package.xml create mode 100644 route_guidance_ros/CMakeLists.txt create mode 100644 route_guidance_ros/package.xml rename {goal_sender => route_guidance_ros}/scripts/Navigation.py (100%) rename {goal_sender => route_guidance_ros}/scripts/Navigation_client.py (100%) rename {goal_sender => route_guidance_ros}/scripts/System_Information.py (100%) rename {goal_sender => route_guidance_ros}/scripts/System_Information_client.py (100%) rename {goal_sender => route_guidance_ros}/scripts/VirtualNavigation.py (100%) rename {goal_sender => route_guidance_ros}/scripts/VirtualNavigation_client.py (100%) rename {goal_sender => route_guidance_ros}/scripts/goal_sender_ros.py (100%) rename goal_sender/scripts/sub_route_guidance_engine.py => route_guidance_ros/scripts/sub_engine.py (98%) rename goal_sender/scripts/sub_route_guidance_engine_client.py => route_guidance_ros/scripts/sub_engine_client.py (100%) rename {goal_sender => route_guidance_ros}/scripts/test_nav_component.py (100%) rename {goal_sender => route_guidance_ros}/scripts/test_sub_engine.py (93%) rename {goal_sender => route_guidance_ros}/scripts/test_sys_component.py (100%) rename {goal_sender => route_guidance_ros}/scripts/test_toplevel_engine.py (86%) rename {goal_sender => route_guidance_ros}/scripts/test_vnav_component.py (80%) rename goal_sender/scripts/toplevel_route_guidance_engine.py => route_guidance_ros/scripts/toplevel_engine.py (99%) rename goal_sender/scripts/toplevel_route_guidance_engine_client.py => route_guidance_ros/scripts/toplevel_engine_client.py (100%) rename {goal_sender => route_guidance_ros}/scripts/utilities.py (86%) diff --git a/goal_sender/CMakeLists.txt b/goal_sender/CMakeLists.txt deleted file mode 100644 index 5a822e32..00000000 --- a/goal_sender/CMakeLists.txt +++ /dev/null @@ -1,206 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(goal_sender) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - actionlib - move_base_msgs - rospy -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# move_base_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES goal_sender -# CATKIN_DEPENDS actionlib move_base_msgs rospy -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/goal_sender.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/goal_sender_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_goal_sender.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/goal_sender/package.xml b/goal_sender/package.xml deleted file mode 100644 index d739126d..00000000 --- a/goal_sender/package.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - goal_sender - 0.0.0 - The goal_sender package - - - - - tanacchi - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - actionlib - move_base_msgs - rospy - actionlib - move_base_msgs - rospy - actionlib - move_base_msgs - rospy - - - - - - - - diff --git a/route_guidance_ros/CMakeLists.txt b/route_guidance_ros/CMakeLists.txt new file mode 100644 index 00000000..d9db1fb2 --- /dev/null +++ b/route_guidance_ros/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 2.8.3) +project(route_guidance_ros) + +find_package(catkin REQUIRED COMPONENTS + actionlib + geometry_msgs + move_base_msgs + rospy +) + +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES route_guidance_ros +# CATKIN_DEPENDS actionlib geometry_msgs move_base_msgs rospy +# DEPENDS system_lib +) + +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) diff --git a/route_guidance_ros/package.xml b/route_guidance_ros/package.xml new file mode 100644 index 00000000..fbc06726 --- /dev/null +++ b/route_guidance_ros/package.xml @@ -0,0 +1,26 @@ + + + route_guidance_ros + 0.0.0 + The route_guidance_ros package + + tanacchi + + MIT + + catkin + actionlib + geometry_msgs + move_base_msgs + rospy + actionlib + geometry_msgs + move_base_msgs + rospy + actionlib + geometry_msgs + move_base_msgs + rospy + + + diff --git a/goal_sender/scripts/Navigation.py b/route_guidance_ros/scripts/Navigation.py similarity index 100% rename from goal_sender/scripts/Navigation.py rename to route_guidance_ros/scripts/Navigation.py diff --git a/goal_sender/scripts/Navigation_client.py b/route_guidance_ros/scripts/Navigation_client.py similarity index 100% rename from goal_sender/scripts/Navigation_client.py rename to route_guidance_ros/scripts/Navigation_client.py diff --git a/goal_sender/scripts/System_Information.py b/route_guidance_ros/scripts/System_Information.py similarity index 100% rename from goal_sender/scripts/System_Information.py rename to route_guidance_ros/scripts/System_Information.py diff --git a/goal_sender/scripts/System_Information_client.py b/route_guidance_ros/scripts/System_Information_client.py similarity index 100% rename from goal_sender/scripts/System_Information_client.py rename to route_guidance_ros/scripts/System_Information_client.py diff --git a/goal_sender/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py similarity index 100% rename from goal_sender/scripts/VirtualNavigation.py rename to route_guidance_ros/scripts/VirtualNavigation.py diff --git a/goal_sender/scripts/VirtualNavigation_client.py b/route_guidance_ros/scripts/VirtualNavigation_client.py similarity index 100% rename from goal_sender/scripts/VirtualNavigation_client.py rename to route_guidance_ros/scripts/VirtualNavigation_client.py diff --git a/goal_sender/scripts/goal_sender_ros.py b/route_guidance_ros/scripts/goal_sender_ros.py similarity index 100% rename from goal_sender/scripts/goal_sender_ros.py rename to route_guidance_ros/scripts/goal_sender_ros.py diff --git a/goal_sender/scripts/sub_route_guidance_engine.py b/route_guidance_ros/scripts/sub_engine.py similarity index 98% rename from goal_sender/scripts/sub_route_guidance_engine.py rename to route_guidance_ros/scripts/sub_engine.py index 33cf9ca9..638b0f79 100644 --- a/goal_sender/scripts/sub_route_guidance_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -1,4 +1,4 @@ -# sub_route_guidance_engine.py +# sub_engine.py # # Copyright 2017 Eiichi Inohira # This software may be modified and distributed under the terms @@ -7,7 +7,7 @@ # For python 3 # For HRI Engine -"""sub_route_guidance_engine +"""sub_engine """ from __future__ import print_function @@ -233,7 +233,7 @@ def test_engine(port): if __name__ == "__main__": if len(sys.argv) < 2: - print("py sub_route_guidance_engine.py ") + print("py sub_engine.py ") raise RuntimeError port = int(sys.argv[1]) diff --git a/goal_sender/scripts/sub_route_guidance_engine_client.py b/route_guidance_ros/scripts/sub_engine_client.py similarity index 100% rename from goal_sender/scripts/sub_route_guidance_engine_client.py rename to route_guidance_ros/scripts/sub_engine_client.py diff --git a/goal_sender/scripts/test_nav_component.py b/route_guidance_ros/scripts/test_nav_component.py similarity index 100% rename from goal_sender/scripts/test_nav_component.py rename to route_guidance_ros/scripts/test_nav_component.py diff --git a/goal_sender/scripts/test_sub_engine.py b/route_guidance_ros/scripts/test_sub_engine.py similarity index 93% rename from goal_sender/scripts/test_sub_engine.py rename to route_guidance_ros/scripts/test_sub_engine.py index a237dd07..a8724fc6 100644 --- a/goal_sender/scripts/test_sub_engine.py +++ b/route_guidance_ros/scripts/test_sub_engine.py @@ -2,7 +2,7 @@ # 2. run this test. -from sub_route_guidance_engine_client import IF as Engine +from sub_engine_client import IF as Engine import time from pyrois.RoIS_Common import Component_Status from utilities import setup_single_robot diff --git a/goal_sender/scripts/test_sys_component.py b/route_guidance_ros/scripts/test_sys_component.py similarity index 100% rename from goal_sender/scripts/test_sys_component.py rename to route_guidance_ros/scripts/test_sys_component.py diff --git a/goal_sender/scripts/test_toplevel_engine.py b/route_guidance_ros/scripts/test_toplevel_engine.py similarity index 86% rename from goal_sender/scripts/test_toplevel_engine.py rename to route_guidance_ros/scripts/test_toplevel_engine.py index 0cc1fe2b..36e2654a 100644 --- a/goal_sender/scripts/test_toplevel_engine.py +++ b/route_guidance_ros/scripts/test_toplevel_engine.py @@ -2,7 +2,7 @@ # 2. run this test. -from toplevel_route_guidance_engine_client import IF as Engine +from toplevel_engine_client import IF as Engine import time from pyrois.RoIS_Common import Component_Status from utilities import setup_multi_robot diff --git a/goal_sender/scripts/test_vnav_component.py b/route_guidance_ros/scripts/test_vnav_component.py similarity index 80% rename from goal_sender/scripts/test_vnav_component.py rename to route_guidance_ros/scripts/test_vnav_component.py index 23947c33..9fbdc7ea 100644 --- a/goal_sender/scripts/test_vnav_component.py +++ b/route_guidance_ros/scripts/test_vnav_component.py @@ -2,18 +2,18 @@ # 2. run this test. -from toplevel_route_guidance_engine_client import IF as Engine +from toplevel_engine_client import IF as Engine import time from pyrois.RoIS_Common import Component_Status from utilities import setup_multi_robot, SubprocessWrapper process = setup_multi_robot() command = [ - "rosrun", "goal_sender", "VirtualNavigation.py", + "rosrun", "route_guidance_ros", "VirtualNavigation.py", "8041", "robot1", "robot2", "robot3" ] process.append(SubprocessWrapper(command)) -time.sleep(5) +time.sleep(10) engine = Engine('http://127.0.0.1:8000') engine.connect() diff --git a/goal_sender/scripts/toplevel_route_guidance_engine.py b/route_guidance_ros/scripts/toplevel_engine.py similarity index 99% rename from goal_sender/scripts/toplevel_route_guidance_engine.py rename to route_guidance_ros/scripts/toplevel_engine.py index 00464d04..c4bcf8b6 100644 --- a/goal_sender/scripts/toplevel_route_guidance_engine.py +++ b/route_guidance_ros/scripts/toplevel_engine.py @@ -166,7 +166,7 @@ class EventIF(RoIS_HRI.EventIF): return (status, results) from VirtualNavigation_client import VirtualNavigation_Client as VNavClient -from sub_route_guidance_engine_client import IF as EngineClient +from sub_engine_client import IF as EngineClient class IF(SystemIF, CommandIF, QueryIF, EventIF): """IF diff --git a/goal_sender/scripts/toplevel_route_guidance_engine_client.py b/route_guidance_ros/scripts/toplevel_engine_client.py similarity index 100% rename from goal_sender/scripts/toplevel_route_guidance_engine_client.py rename to route_guidance_ros/scripts/toplevel_engine_client.py diff --git a/goal_sender/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py similarity index 86% rename from goal_sender/scripts/utilities.py rename to route_guidance_ros/scripts/utilities.py index 9fa84a5f..87ccf0d0 100644 --- a/goal_sender/scripts/utilities.py +++ b/route_guidance_ros/scripts/utilities.py @@ -12,7 +12,7 @@ class SubprocessWrapper(object): class ComponentWrapper(SubprocessWrapper): def __init__(self, component_name, robot_name, port): - super().__init__(["rosrun", "goal_sender", component_name, str(port), robot_name]) + super().__init__(["rosrun", "route_guidance_ros", component_name, str(port), robot_name]) class NavigationComponent(ComponentWrapper): @@ -27,12 +27,12 @@ class SysInfoComponent(ComponentWrapper): class SubEngineWrapper(SubprocessWrapper): def __init__(self, engine_port): - super().__init__(["python3", "sub_route_guidance_engine.py", str(engine_port)]) + super().__init__(["python3", "sub_engine.py", str(engine_port)]) class ToplevelEngineWrapper(SubprocessWrapper): def __init__(self): - super().__init__(["python3", "toplevel_route_guidance_engine.py"]) + super().__init__(["python3", "toplevel_engine.py"]) def launch_components_and_subengines(**robot_port_table): -- GitLab