diff --git a/route_guidance_ros/scripts/Navigation.py b/route_guidance_ros/scripts/Navigation.py index 593f1fc51f1be970074315a78ab8d245d4f11771..453125156563fd9c6a29ce8acf0e27939b85e30f 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 0abc14dfab16fc9b36468bfdc0c98e87d1cb536d..3d7d651bc106fe712e708067e27520a5c7b59c4a 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,17 @@ 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 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) + 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)) @@ -100,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) @@ -130,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 new file mode 100644 index 0000000000000000000000000000000000000000..4f527735ef61a6926a969d4362ef3d022dc091d3 --- /dev/null +++ b/route_guidance_ros/scripts/field_points.yaml @@ -0,0 +1,10 @@ +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 d7f6f355f14ec05c619b6c85f8b880277fe1b8da..13bec919b398c04319d4ba86118f6f66dff21b04 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 702500a81dda3a1e653d9cfacf327410c9353323..63fbcee9bf25f8db6d6cc2c5aa11d973d9934d61 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -207,7 +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] - 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 e16f7de0bdfcfa6ed0f34b5420664b0319c4d08b..4e33277f65f672837f74917fd91a4212e0faba70 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 53750282e9f0fb41095c59520e7234c366d18e73..d2919b850bf5cf709b73537a45d1292629bada5f 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 a218e41abab339c7ecc9ae05770c5a8c4242e68c..73e97a1cda691af9de30f4b45fc2883964334dfe 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, 0], "point_e", "point_a", [0, 5, 0] ] 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_nav_component.py b/route_guidance_ros/scripts/test_nav_component.py index 8724364d6d422fe0835685c5f8d5eaf4921901ac..a01f387e6aa5b4f6a87f8b1f3dc1626803293a2a 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 668dd648e9ed017b45ab74fae682b8614ae4e9bc..9b72c42a02dac0243b554d2626b4dc2c2376a1a4 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 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 85d9de4830b959363a47f34224be3a3bd90a0032..f437db9cb8007c0ff1b813ac93544cd304ba59de 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: