From 8c25836a9e759223994074679eba3244f04c72e5 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 1 Dec 2019 17:57:23 +0900 Subject: [PATCH 1/4] Add file for field point name --- route_guidance_ros/scripts/field_points.yaml | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 route_guidance_ros/scripts/field_points.yaml diff --git a/route_guidance_ros/scripts/field_points.yaml b/route_guidance_ros/scripts/field_points.yaml new file mode 100644 index 00000000..37087258 --- /dev/null +++ b/route_guidance_ros/scripts/field_points.yaml @@ -0,0 +1,10 @@ +point_a: [ 25, 20 ] +point_b: [ 25, 10 ] +point_c: [ 25, 0 ] +point_d: [ 15, 0 ] +point_e: [ 10, 0 ] +point_f: [ 5, 20 ] +point_g: [ 0, 0 ] +point_h: [ 0, 10 ] +point_i: [ 0, 20 ] +point_j: [ 0, 20 ] -- GitLab From 7e13ae45a2259dc57289959f5fd85b7af6e0356d Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 1 Dec 2019 19:01:52 +0900 Subject: [PATCH 2/4] Modify component's set_parameter to use named-point --- route_guidance_ros/scripts/Navigation.py | 6 ++++-- route_guidance_ros/scripts/VirtualNavigation.py | 4 ++++ 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/Navigation.py b/route_guidance_ros/scripts/Navigation.py index 593f1fc5..45312515 100755 --- a/route_guidance_ros/scripts/Navigation.py +++ b/route_guidance_ros/scripts/Navigation.py @@ -72,7 +72,7 @@ class Event(RoIS_Common.Event): from goal_sender_ros import GoalSenderROS import xmlrpclib - +import yaml from pyrois.RoIS_Common import Component_Status class Navigation(Event, Command, Query): @@ -84,11 +84,13 @@ class Navigation(Event, Command, Query): self._component.Time_Limit = 10 self._component.Routing_Policy = "" self.latest_nav_state = Component_Status.UNINITIALIZED + self.pointname_coord_table = yaml.load(open("field_points.yaml")) def set_parameter(self, target_position, time_limit, routing_policy): # Put event self.latest_nav_state = Component_Status.BUSY - + if isinstance(target_position, str) and target_position in self.pointname_coord_table: + target_position = self.pointname_coord_table[target_position] self._goal_sender.send_goal(target_position) status = RoIS_HRI.ReturnCode_t.OK.value return status diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 0abc14df..3ed752df 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -75,6 +75,7 @@ class Event(RoIS_Common.Event): from goal_sender_ros import GoalSenderROS from pyrois.RoIS_Common import Component_Status +import yaml import xmlrpclib @@ -88,9 +89,12 @@ class VirtualNavigation(Event, Command, Query): self.robot_goal_table = {} self.nav_state_table = \ {robot_name : Component_Status.UNINITIALIZED for robot_name in robot_names} + self.pointname_coord_table = yaml.load(open("field_points.yaml")) def set_parameter(self, target_position, time_limit, routing_policy): nearest_robot = self._component.get_nearest_robotname(target_position) + if isinstance(target_position, str) and target_position in self.pointname_coord_table: + target_position = self.pointname_coord_table[target_position] th = threading.Thread( target=self.send_goal_and_event, args=(target_position, nearest_robot)) -- GitLab From 09dfcd5212006d7ee34eebf3290747fd95f81f8a Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 2 Dec 2019 01:57:14 +0900 Subject: [PATCH 3/4] Use named-points for navigation service --- route_guidance_ros/scripts/VirtualNavigation.py | 14 ++++++++++---- route_guidance_ros/scripts/main_engine.py | 3 ++- .../scripts/test_main_engine_with_service.py | 10 +++++----- .../scripts/test_sub_engine_with_service.py | 2 +- 4 files changed, 18 insertions(+), 11 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 3ed752df..1609ccff 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -91,10 +91,15 @@ class VirtualNavigation(Event, Command, Query): {robot_name : Component_Status.UNINITIALIZED for robot_name in robot_names} self.pointname_coord_table = yaml.load(open("field_points.yaml")) + def position_to_coord(self, position): + if isinstance(position, str) and position in self.pointname_coord_table: + return self.pointname_coord_table[position] + else: + return position + def set_parameter(self, target_position, time_limit, routing_policy): - nearest_robot = self._component.get_nearest_robotname(target_position) - if isinstance(target_position, str) and target_position in self.pointname_coord_table: - target_position = self.pointname_coord_table[target_position] + target_coord = self.position_to_coord(target_position) + nearest_robot = self._component.get_nearest_robotname(target_coord) th = threading.Thread( target=self.send_goal_and_event, args=(target_position, nearest_robot)) @@ -104,7 +109,8 @@ class VirtualNavigation(Event, Command, Query): return status def send_goal_and_event(self, goal, robot_name): - status = self._component.send_goal_to_robot_async(goal, robot_name) + goal_coord = self.position_to_coord(goal) + status = self._component.send_goal_to_robot_async(goal_coord, robot_name) msg = xmlrpclib.dumps((goal, robot_name), 'completed') self.event_queue.put(msg) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 702500a8..65814e1a 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -207,7 +207,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): target_component = self.component_clients[component_ref] status = target_component.set_parameter(*parameters).value goal = parameters[0] - goal.append(0) + if not isinstance(goal, str): + goal.append(0) self.goal_command_table[str(goal)] = str(self.command_id) elif component_ref == 'RobotPosition': self.command_id_result_table[str(self.command_id)] = [] diff --git a/route_guidance_ros/scripts/test_main_engine_with_service.py b/route_guidance_ros/scripts/test_main_engine_with_service.py index a218e41a..3abd6aa3 100644 --- a/route_guidance_ros/scripts/test_main_engine_with_service.py +++ b/route_guidance_ros/scripts/test_main_engine_with_service.py @@ -49,15 +49,15 @@ class Service(Service_Application_IF): def run(self): destinations = [ - [25, 10], [0, 20], [5, 0], [10, 0], [25, 20], [0, 5] + "point_b", "point_h", [5, 0], "point_e", "point_a", [0, 5] ] for dest in destinations: self.go_robot_to(dest) while self.is_running: - self.update_robot_positions() - robot_positions = self.get_robot_positions() - print("robot_positions:") - [print(f"\t{i}: {pos}") for i, pos in enumerate(robot_positions)] + # self.update_robot_positions() + # robot_positions = self.get_robot_positions() + # print("robot_positions:") + # [print(f"\t{i}: {pos}") for i, pos in enumerate(robot_positions)] time.sleep(3) diff --git a/route_guidance_ros/scripts/test_sub_engine_with_service.py b/route_guidance_ros/scripts/test_sub_engine_with_service.py index 85d9de48..f437db9c 100644 --- a/route_guidance_ros/scripts/test_sub_engine_with_service.py +++ b/route_guidance_ros/scripts/test_sub_engine_with_service.py @@ -45,7 +45,7 @@ class Service(Service_Application_IF): print("----------------------------------------------") def run(self): - destinations = [[5, 0], [10, 0], [25, 0], [25, 20]] + destinations = ["point_e", "point_d", "point_c", "point_b"] for dest in destinations: self.go_robot_to(dest) while self.is_navigation_running: -- GitLab From 731dbeb05254651eb914dcac9bfa8f2ea7d0d5da Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 2 Dec 2019 02:16:50 +0900 Subject: [PATCH 4/4] Change shape of coord 2D => 3D --- .../scripts/VirtualNavigation.py | 2 +- route_guidance_ros/scripts/field_points.yaml | 20 +++++++++---------- route_guidance_ros/scripts/goal_sender_ros.py | 2 -- route_guidance_ros/scripts/main_engine.py | 2 -- route_guidance_ros/scripts/test_app.py | 2 +- .../scripts/test_main_engine.py | 2 +- .../scripts/test_main_engine_with_service.py | 2 +- .../scripts/test_nav_component.py | 2 +- route_guidance_ros/scripts/test_sub_engine.py | 2 +- 9 files changed, 16 insertions(+), 20 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 1609ccff..3d7d651b 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -140,7 +140,7 @@ class Component(object): 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]) + self.__robot_positions[robot_name] = np.array([pos.x, pos.y, pos.z]) def send_goal_to_nearest_robot(self, dest): dest = np.array(dest) diff --git a/route_guidance_ros/scripts/field_points.yaml b/route_guidance_ros/scripts/field_points.yaml index 37087258..4f527735 100644 --- a/route_guidance_ros/scripts/field_points.yaml +++ b/route_guidance_ros/scripts/field_points.yaml @@ -1,10 +1,10 @@ -point_a: [ 25, 20 ] -point_b: [ 25, 10 ] -point_c: [ 25, 0 ] -point_d: [ 15, 0 ] -point_e: [ 10, 0 ] -point_f: [ 5, 20 ] -point_g: [ 0, 0 ] -point_h: [ 0, 10 ] -point_i: [ 0, 20 ] -point_j: [ 0, 20 ] +point_a: [ 25, 20, 0 ] +point_b: [ 25, 10, 0 ] +point_c: [ 25, 0, 0 ] +point_d: [ 15, 0, 0 ] +point_e: [ 10, 0, 0 ] +point_f: [ 5, 20, 0 ] +point_g: [ 0, 0, 0 ] +point_h: [ 0, 10, 0 ] +point_i: [ 0, 20, 0 ] +point_j: [ 0, 20, 0 ] diff --git a/route_guidance_ros/scripts/goal_sender_ros.py b/route_guidance_ros/scripts/goal_sender_ros.py index d7f6f355..13bec919 100644 --- a/route_guidance_ros/scripts/goal_sender_ros.py +++ b/route_guidance_ros/scripts/goal_sender_ros.py @@ -43,13 +43,11 @@ class GoalSenderROS(object): self.action_client.wait_for_server() def send_goal(self, pose_xy): - pose_xy.append(0) pose = [pose_xy, (0.0, 0.0, 0.0, 1.0)] goal = pose_to_goal(pose) self.action_client.send_goal(goal) def send_goal_and_wait(self, pose_xy): - pose_xy.append(0) pose = [pose_xy, (0.0, 0.0, 0.0, 1.0)] goal = pose_to_goal(pose) return self.action_client.send_goal_and_wait(goal) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 65814e1a..63fbcee9 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -207,8 +207,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): target_component = self.component_clients[component_ref] status = target_component.set_parameter(*parameters).value goal = parameters[0] - if not isinstance(goal, str): - goal.append(0) self.goal_command_table[str(goal)] = str(self.command_id) elif component_ref == 'RobotPosition': self.command_id_result_table[str(self.command_id)] = [] diff --git a/route_guidance_ros/scripts/test_app.py b/route_guidance_ros/scripts/test_app.py index e16f7de0..4e33277f 100644 --- a/route_guidance_ros/scripts/test_app.py +++ b/route_guidance_ros/scripts/test_app.py @@ -13,7 +13,7 @@ time.sleep(10) print("launching Service...") service = Service('http://localhost:8000') print("Service was established.") -for dest in [[25, 0], [0, 20], [5, 0]]: +for dest in [[25, 0, 0], [0, 20, 0], [5, 0, 0]]: status = service.go_robot_to(dest) print(status) time.sleep(5) diff --git a/route_guidance_ros/scripts/test_main_engine.py b/route_guidance_ros/scripts/test_main_engine.py index 53750282..d2919b85 100644 --- a/route_guidance_ros/scripts/test_main_engine.py +++ b/route_guidance_ros/scripts/test_main_engine.py @@ -13,7 +13,7 @@ engine = Engine('http://127.0.0.1:8000') print("Engine was established.") time.sleep(10) engine.connect() -for dest in [[25, 0], [0, 20], [5, 0]]: +for dest in [[25, 0, 0], [0, 20, 0], [5, 0, 0]]: status = engine.set_parameter('Navigation', dest, "", "") print(status) time.sleep(5) diff --git a/route_guidance_ros/scripts/test_main_engine_with_service.py b/route_guidance_ros/scripts/test_main_engine_with_service.py index 3abd6aa3..73e97a1c 100644 --- a/route_guidance_ros/scripts/test_main_engine_with_service.py +++ b/route_guidance_ros/scripts/test_main_engine_with_service.py @@ -49,7 +49,7 @@ class Service(Service_Application_IF): def run(self): destinations = [ - "point_b", "point_h", [5, 0], "point_e", "point_a", [0, 5] + "point_b", "point_h", [5, 0, 0], "point_e", "point_a", [0, 5, 0] ] for dest in destinations: self.go_robot_to(dest) diff --git a/route_guidance_ros/scripts/test_nav_component.py b/route_guidance_ros/scripts/test_nav_component.py index 8724364d..a01f387e 100644 --- a/route_guidance_ros/scripts/test_nav_component.py +++ b/route_guidance_ros/scripts/test_nav_component.py @@ -13,4 +13,4 @@ client = Navigation_Client(uri) print(client.start()) time.sleep(3) -print(client.set_parameter([10, 0], "", "")) +print(client.set_parameter([10, 0, 0], "", "")) diff --git a/route_guidance_ros/scripts/test_sub_engine.py b/route_guidance_ros/scripts/test_sub_engine.py index 668dd648..9b72c42a 100644 --- a/route_guidance_ros/scripts/test_sub_engine.py +++ b/route_guidance_ros/scripts/test_sub_engine.py @@ -12,7 +12,7 @@ process = setup_single_robot() engine = Engine('http://127.0.0.1:8010') engine.connect() engine.search("engine_profile_test") -destinations = [[5, 0], [10, 0], [25, 0], [25, 10]] +destinations = [[5, 0, 0], [10, 0, 0], [25, 0, 0], [25, 10, 0]] for dest in destinations: print(engine.set_parameter('Navigation', dest, "", "")) navi_status = Component_Status.BUSY -- GitLab