diff --git a/rois_description/robot/human1.urdf.xacro b/rois_description/robot/human1.urdf.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..d367df1ed7873d3c9942fcf0f21eed7e49a0f6ce
--- /dev/null
+++ b/rois_description/robot/human1.urdf.xacro
@@ -0,0 +1,185 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/VelocityJointInterface
+
+
+ hardware_interface/VelocityJointInterface
+ 15
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/VelocityJointInterface
+
+
+ hardware_interface/VelocityJointInterface
+ 15
+
+
+
+
+
+
+ human1
+
+
+
+
+ true
+ Gazebo/Red
+
+
+
+
+
+ true
+ Gazebo/White
+
+
+
+
+
+ true
+ Gazebo/White
+
+
+
+
+
+ true
+ Gazebo/Black
+
+
+
+
+
diff --git a/rois_gazebo/config/human1_controller.yaml b/rois_gazebo/config/human1_controller.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ddf31bf486d4d09bde15875bda27a110713fcd34
--- /dev/null
+++ b/rois_gazebo/config/human1_controller.yaml
@@ -0,0 +1,46 @@
+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: human1/base_footprint #default: base_link
+ odom_frame_id: human1/odom
+
+ # 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 95e9dfdb0d4d19be77bad385f3b2110662cc04d3..99fb925a5a4ef0d774e57621c90bc1ef27355eba 100644
--- a/rois_gazebo/launch/setup_gazebo.launch
+++ b/rois_gazebo/launch/setup_gazebo.launch
@@ -15,9 +15,9 @@
value="$(arg world_name)" />
-
+
-
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/rois_gazebo/launch/spawn_robot.launch b/rois_gazebo/launch/spawn_model.launch
similarity index 72%
rename from rois_gazebo/launch/spawn_robot.launch
rename to rois_gazebo/launch/spawn_model.launch
index 9846c6640aaef168aff018b8dd8d6ec587670c06..7af09e7b7195278ab0e94c7a81207cc82525a58e 100644
--- a/rois_gazebo/launch/spawn_robot.launch
+++ b/rois_gazebo/launch/spawn_model.launch
@@ -3,21 +3,21 @@
+ default="$(find rois_description)/robot/$(arg model_name).urdf.xacro" />
-
+
-
+
-
+
+ args="-urdf -model $(arg model_name) -param robot_description $(arg init_pos)" />
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/System_Information.py b/route_guidance_ros/scripts/System_Information.py
index a2493cc13731ed606f8a1355b792b24ee4a2bf68..b7370ad2c41667b6bfa7ab30709b2a7c73b8f16b 100755
--- a/route_guidance_ros/scripts/System_Information.py
+++ b/route_guidance_ros/scripts/System_Information.py
@@ -60,14 +60,15 @@ class Query(RoIS_Common.Query):
class System_Information(Query):
"""System_Information
"""
- def __init__(self, c):
+ def __init__(self, c, robot_name):
super(System_Information, self).__init__(c)
+ self._robot_name = robot_name
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
+ robot_ref = ["urn:x-rois:def:HRIComponent:Kyutech::{}".format(self._robot_name)]
position_data = self._component.get_position()
return (status, timestamp, robot_ref, position_data)
@@ -98,7 +99,7 @@ def example_si(port, robot_name):
"""example_si
"""
c = component(robot_name)
- si = System_Information(c)
+ si = System_Information(c, robot_name)
# start the timer to dispatch events
t = threading.Timer(0.1, event_dispatch, args=(si,))
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 f76f9e9df4e9436e1dda4d99c33de02c2a3c6286..63fbcee9bf25f8db6d6cc2c5aa11d973d9934d61 100644
--- a/route_guidance_ros/scripts/main_engine.py
+++ b/route_guidance_ros/scripts/main_engine.py
@@ -197,34 +197,40 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base):
self.goal_command_table = {}
for engine_client in self.engine_clients.values():
engine_client.connect()
+ self.command_id_result_table = {}
+ th = threading.Thread(target=self.analyze_vnav_status, daemon=True)
+ th.start()
- def set_parameter(self, component_ref, parameters):
- target_component = self.component_clients[component_ref]
+ def set_parameter(self, component_ref, *parameters):
self.command_id += 1
- status = target_component.set_parameter(*parameters).value
- th = threading.Thread(
- target=self.analyze_c_status,
- daemon=True,
- args=(self.component_clients[component_ref], str(self.command_id)))
- th.start()
- goal = parameters[0]
- goal.append(0)
- self.goal_command_table[str(goal)] = str(self.command_id)
+ if component_ref == 'Navigation':
+ target_component = self.component_clients[component_ref]
+ status = target_component.set_parameter(*parameters).value
+ goal = parameters[0]
+ self.goal_command_table[str(goal)] = str(self.command_id)
+ elif component_ref == 'RobotPosition':
+ self.command_id_result_table[str(self.command_id)] = []
+ for engine_client in self.engine_clients.values():
+ status, command_id = engine_client.set_parameter('System_Information')
+ time.sleep(3)
+ status, results = engine_client.get_command_result(command_id, "")
+ self.command_id_result_table[str(self.command_id)].append(results)
+ status = RoIS_HRI.ReturnCode_t.OK.value
return (status, str(self.command_id))
- def analyze_c_status(self, component, command_id):
+ def get_command_result(self, command_id, condition):
+ status = RoIS_HRI.ReturnCode_t.OK.value
+ return status, self.command_id_result_table[command_id]
+
+ def analyze_vnav_status(self):
while True:
- # Is component_event_queue thread-safe ?
msg = self.component_event_queue.get()
(target_pos, robot_name), methodname = xmlrpc.client.loads(msg)
- if self.goal_command_table[str(target_pos)] == command_id:
- print("Command (id: {}) was completed by {} which reached goal {}."\
- .format(command_id, robot_name, target_pos))
- self.completed(command_id, Component_Status.READY.value)
- del self.goal_command_table[str(target_pos)]
- return
- else:
- self.component_event_queue.put(msg)
+ command_id = self.goal_command_table[str(target_pos)]
+ print("Command (id: {}) was completed by {} which reached goal {}."\
+ .format(command_id, robot_name, target_pos))
+ self.completed(command_id, Component_Status.READY.value)
+ del self.goal_command_table[str(target_pos)]
time.sleep(3)
diff --git a/route_guidance_ros/scripts/main_engine_client.py b/route_guidance_ros/scripts/main_engine_client.py
index 1e7e3a18b65ac98a06f42a93214b0e1f2aa21ad6..3d9822794b9c40115eaaad5eb84f6eceb7f37d65 100644
--- a/route_guidance_ros/scripts/main_engine_client.py
+++ b/route_guidance_ros/scripts/main_engine_client.py
@@ -69,8 +69,8 @@ class CommandIF(RoIS_HRI.CommandIF):
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)
+ 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)
diff --git a/route_guidance_ros/scripts/service_app.py b/route_guidance_ros/scripts/service_app.py
deleted file mode 100644
index fc0a61466d5292eba2167273a070718442cbce40..0000000000000000000000000000000000000000
--- a/route_guidance_ros/scripts/service_app.py
+++ /dev/null
@@ -1,95 +0,0 @@
-from main_engine_client import IF as RoIS_IF
-from pyrois.Service_Application_IF import Service_Application_IF as pyrois_Service_Application_IF
-from pyrois import RoIS_Service, RoIS_HRI
-
-import logging
-import queue
-import threading
-import time
-
-import socketserver
-import xmlrpc.server
-
-
-class Service_Application_IF(pyrois_Service_Application_IF):
- """Service_Application_IF
- """
- def __init__(self, uri, logger=None):
- self._uri = uri
- self._proxy = xmlrpc.client.ServerProxy(self._uri)
- self.command_id_table = {}
- if logger is not None:
- self.logger = logger
- self.th = threading.Thread(target=self.event_poll_loop)
- self.th.start()
-
- def event_poll_loop(self):
- print("poll loop started")
- while True:
- # try:
- # self.poll_event()
- self.poll_event()
- # except:
- # print("Error ")
- # continue
-
- def poll_event(self):
- """poll_event
- """
- msg = self._proxy.poll_event()
- if msg is None:
- return
- (params, methodname) = xmlrpc.client.loads(msg)
- #self.logger.debug('poll_event: '+methodname)
- print("params: ", params, ", methodname: ", methodname)
- if methodname == 'completed':
- self.completed(params[0], params[1])
- elif methodname == 'notify_error':
- self.notify_error(params[0], params[1])
- elif methodname == 'notify_event':
- self.notify_event(params[0], params[1], params[2], params[3])
-
- def completed(self, command_id, status):
- self.command_id_table[command_id] = True
- print(self.command_id_table)
-
- def notify_error(self, error_id, error_type):
- print('received error event {}({}) ',
- error_id,
- RoIS_Service.ErrorType(error_type).name)
-
- def notify_event(self, event_id, event_type, subscribe_id, expire):
- print('received event {} {} {} {}',
- event_id,
- event_type,
- subscribe_id,
- expire)
-
- def go_robot_to(self, dest):
- self._proxy = xmlrpc.client.ServerProxy(self._uri)
- time.sleep(3)
- (return_code, command_id) = self._proxy.set_parameter('Navigation', [dest, "", ""])
- stautus = RoIS_HRI.ReturnCode_t(return_code)
- self.command_id_table[command_id] = False
- print(self.command_id_table)
-
-
-def example_sa_IF(url, q):
- try:
- logger = logging.getLogger('Service_Application_IF')
- logger.setLevel(logging.DEBUG)
- ch = logging.handlers.QueueHandler(q)
- # ch = logging.StreamHandler()
- ch.setLevel(logging.DEBUG)
- formatter = logging.Formatter(
- '%(asctime)s - %(name)s - %(levelname)s - %(message)s')
- ch.setFormatter(formatter)
- logger.addHandler(ch)
- a = Service_Application_IF(url, logger=logger)
- except KeyboardInterrupt:
- print("Interrupted")
-
-
-if __name__ == '__main__':
- q = queue.Queue()
- example_sa_IF("http://localhost:8000", q)
diff --git a/route_guidance_ros/scripts/service_app_lv1.py b/route_guidance_ros/scripts/service_app_lv1.py
new file mode 100644
index 0000000000000000000000000000000000000000..5a220f816617ce71873608e6f9505e8d548de31f
--- /dev/null
+++ b/route_guidance_ros/scripts/service_app_lv1.py
@@ -0,0 +1,88 @@
+from pyrois.Service_Application_IF import Service_Application_IF
+from pyrois import RoIS_Service, RoIS_HRI
+from utilities import setup_multi_robot
+from pyrois.RoIS_Common import Component_Status
+
+import logging
+import queue
+import threading
+import time
+import enum
+import xmlrpc
+
+
+class TaskKind(enum.Enum):
+ NaviToUser = 1
+ NaviToGuide = 2
+ End = 3
+
+
+class TaskState(object):
+ __task_seq_table = {
+ TaskKind.NaviToUser: TaskKind.NaviToGuide,
+ TaskKind.NaviToGuide: TaskKind.End,
+ TaskKind.End: TaskKind.End
+ }
+
+ def __init__(self, task_kind, status=Component_Status.BUSY):
+ self.task_kind = task_kind
+ self.status = status
+
+ def __repr__(self):
+ return "".format(self.task_kind, self.status)
+
+ def next(self):
+ return TaskState.__task_seq_table[self.task_kind]
+
+
+class Service(Service_Application_IF):
+ def __init__(self, uri="http://localhost:8000", logger=None):
+ super().__init__(uri)
+ self.target_pos = "point_c"
+ self.dest_pos = "point_a"
+ self.commandid_taskstate_table = {}
+
+ def completed(self, command_id, status):
+ task = self.commandid_taskstate_table[command_id]
+ status = Component_Status(status)
+ task.status = status
+ next_task = task.next()
+ if next_task == TaskKind.NaviToGuide:
+ self.start_navi_to_guide(self.dest_pos)
+ elif next_task == TaskKind.End:
+ print("Route Guide Finished !!")
+ self.commandid_taskstate_table[command_id] = task
+ print(self.commandid_taskstate_table)
+
+ def notify_error(self, error_id, error_type):
+ print('received error event {}({}) '.format( \
+ error_id, RoIS_Service.ErrorType(error_type).name))
+
+ def notify_event(self, event_id, event_type, subscribe_id, expire):
+ print('received event {} {} {} {}'.format( \
+ event_id, event_type, subscribe_id, expire))
+
+ def start_navi_to_user(self, dest):
+ (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "")
+ stautus = RoIS_HRI.ReturnCode_t(return_code)
+ self.commandid_taskstate_table[command_id] = TaskState(TaskKind.NaviToUser)
+
+ def start_navi_to_guide(self, dest):
+ (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "")
+ stautus = RoIS_HRI.ReturnCode_t(return_code)
+ self.commandid_taskstate_table[command_id] = TaskState(TaskKind.NaviToGuide)
+
+ def run(self):
+ self._proxy = xmlrpc.client.ServerProxy(self._uri)
+ time.sleep(3)
+ target_point = "point_c"
+ self.start_navi_to_user(target_point)
+
+
+if __name__ == '__main__':
+ process = setup_multi_robot()
+ time.sleep(5)
+ service = Service()
+ print("Starting service..")
+ service.run()
+ print("Finish.")
diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py
index 6a90c2524324b93a521ef34f169c09d98fe4b920..e9215bf1c3c74e1bc7c4891fb9dcb23dfca12c28 100644
--- a/route_guidance_ros/scripts/sub_engine.py
+++ b/route_guidance_ros/scripts/sub_engine.py
@@ -184,20 +184,34 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base):
'Navigation': NavClient('http://localhost:' + str(engine_port+1)),
'System_Information': SysClient('http://localhost:' + str(engine_port+2))
}
+ self.command_id_status_table = {}
- def set_parameter(self, component_ref, parameters):
+ def set_parameter(self, component_ref, *parameters):
status = None
self.command_id += 1
if not component_ref in self.compoent_clients:
status = RoIS_HRI.ReturnCode_t.ERROR.value
else:
- print("[set_parameter] component_ref started")
target_component_client = self.compoent_clients[component_ref]
- status = target_component_client.set_parameter(*parameters).value
- th = threading.Thread(target=self.analyze_c_status, daemon=True, args=(target_component_client, str(self.command_id)))
- th.start()
+ if component_ref == 'Navigation':
+ status = target_component_client.set_parameter(*parameters).value
+ th = threading.Thread(
+ target=self.analyze_c_status,
+ daemon=True,
+ args=(target_component_client, str(self.command_id)))
+ th.start()
+ elif component_ref == 'System_Information':
+ status = RoIS_HRI.ReturnCode_t.OK.value
+ result = target_component_client.robot_position()
+ result = (result[0].value, result[1], result[2], result[3]) # FIXME
+ self.command_id_status_table[str(self.command_id)] = result
return (status, str(self.command_id))
+ def get_command_result(self, command_id, condition):
+ status = RoIS_HRI.ReturnCode_t.OK.value
+ results = self.command_id_status_table[command_id]
+ return (status, results)
+
### Remove ###
def get_navi_status(self):
status, c_status = self.compoent_clients['Navigation'].component_status()
diff --git a/route_guidance_ros/scripts/sub_engine_client.py b/route_guidance_ros/scripts/sub_engine_client.py
index 0fc6e7ece7b008f0f58d450135182e38db52cb4a..04f0b12a9c5193187d8c109f52a9d7d80b1f0595 100644
--- a/route_guidance_ros/scripts/sub_engine_client.py
+++ b/route_guidance_ros/scripts/sub_engine_client.py
@@ -71,8 +71,8 @@ class CommandIF(RoIS_HRI.CommandIF):
status = RoIS_HRI.ReturnCode_t(s)
return (status, parameter_list)
- def set_parameter(self, component_ref, parameters):
- (status, command_id) = self._proxy.set_parameter(component_ref, parameters)
+ def set_parameter(self, component_ref, *parameters):
+ (status, command_id) = self._proxy.set_parameter(component_ref, *parameters)
status = RoIS_HRI.ReturnCode_t(status)
return (status, 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 c00e159b8c9a0552f54bde03ef1070b2a5a1c0d9..d2919b850bf5cf709b73537a45d1292629bada5f 100644
--- a/route_guidance_ros/scripts/test_main_engine.py
+++ b/route_guidance_ros/scripts/test_main_engine.py
@@ -13,8 +13,8 @@ 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]]:
- status = engine.set_parameter('Navigation', [dest, "", ""])
+for dest in [[25, 0, 0], [0, 20, 0], [5, 0, 0]]:
+ status = engine.set_parameter('Navigation', dest, "", "")
print(status)
time.sleep(5)
# engine.all_finished()
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 60718ad2c0addde7796fca415b8dae155c8179f9..73e97a1cda691af9de30f4b45fc2883964334dfe 100644
--- a/route_guidance_ros/scripts/test_main_engine_with_service.py
+++ b/route_guidance_ros/scripts/test_main_engine_with_service.py
@@ -15,26 +15,49 @@ class Service(Service_Application_IF):
super().__init__(uri)
self.current_dest_index = 0
self.is_running = False
+ self.robot_positions = []
+ self.commandid_task_table = {}
def completed(self, command_id, status):
print("Service command_id {} was completed.".format(command_id))
- self.current_dest_index += 1
- self.is_running = False
+ if self.commandid_task_table[command_id] == 'Navigation':
+ self.current_dest_index += 1
+ self.is_running = False
+ else: # task 'RobotPosition'
+ status, results = self._proxy.get_command_result(command_id, None)
+ self.robot_positions = results
def go_robot_to(self, dest):
self._proxy = xmlrpc.client.ServerProxy(self._uri)
time.sleep(3)
- (return_code, command_id) = self._proxy.set_parameter('Navigation', [dest, "", ""])
+ (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "")
+ self.commandid_task_table[command_id] = 'Navigation'
print("go_robot_to {} called with command_id {}".format(dest, command_id))
self.is_running = True
+ def update_robot_positions(self):
+ self._proxy = xmlrpc.client.ServerProxy(self._uri)
+ time.sleep(3)
+ (return_code, command_id) = self._proxy.set_parameter('RobotPosition')
+ self.commandid_task_table[command_id] = 'RobotPosition'
+ time.sleep(2)
+ status, results = self._proxy.get_command_result(command_id, "")
+ self.robot_positions = results
+
+ def get_robot_positions(self):
+ return self.robot_positions
+
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)]
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 c6b6aad0a458b90e3664b8214c9b8a0accaddc28..9b72c42a02dac0243b554d2626b4dc2c2376a1a4 100644
--- a/route_guidance_ros/scripts/test_sub_engine.py
+++ b/route_guidance_ros/scripts/test_sub_engine.py
@@ -12,9 +12,9 @@ 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, "", ""]))
+ print(engine.set_parameter('Navigation', dest, "", ""))
navi_status = Component_Status.BUSY
while navi_status != Component_Status.READY:
# server_status, navi_status = engine.get_navi_status()
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 01f0dc3951f905ae562a4ff5ee7ff3513eeb6bb5..a0e9d53db73a7c1cf48322845a364a1e106a8672 100644
--- a/route_guidance_ros/scripts/test_sub_engine_with_service.py
+++ b/route_guidance_ros/scripts/test_sub_engine_with_service.py
@@ -7,7 +7,7 @@ import time
from pyrois.RoIS_Common import Component_Status
from utilities import setup_single_robot
from collections import namedtuple
-from service_app import Service_Application_IF
+from pyrois.Service_Application_IF import Service_Application_IF
import xmlrpc
@@ -38,14 +38,14 @@ class Service(Service_Application_IF):
self._proxy = xmlrpc.client.ServerProxy(self._uri)
time.sleep(3)
status, command_id = \
- self._proxy.set_parameter('Navigation',[dest, "", ""])
+ self._proxy.set_parameter('Navigation', dest, "", "")
self.commmand_status_table[command_id] = CommandStatus(False)
print("----------------------------------------------")
print("commmand_status:\n", self.commmand_status_table)
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:
diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py
index 8ff7edbd8686917b336cdb9dcef6de79c8e63e0f..0802b5e43b7db97165fbddff7ce1e4b35920e81b 100644
--- a/route_guidance_ros/scripts/utilities.py
+++ b/route_guidance_ros/scripts/utilities.py
@@ -1,13 +1,22 @@
import time
+import subprocess
from subprocess import Popen
+import atexit
+def register_process_killer(process):
+ def kill_subprocess(pid):
+ subprocess.run(["kill", "-SIGKILL", str(pid)])
+ print("process {} was killed.".format(pid))
+ atexit.register(kill_subprocess, process.pid)
class SubprocessWrapper(object):
def __init__(self, command):
self.process = Popen(command)
+ register_process_killer(self.process)
- def __del__(self):
- self.process.kill()
+ # It is not guaranteed that __del__() methods are called when the interpreter exits.
+ # def __del__(self):
+ # self.process.kill()
class ComponentWrapper(SubprocessWrapper):