diff --git a/examples/hospitalSimulationRos/hospital_ros_run.py b/examples/hospitalSimulationRos/hospital_ros_run.py index 3bd9a17..de4a0d9 100644 --- a/examples/hospitalSimulationRos/hospital_ros_run.py +++ b/examples/hospitalSimulationRos/hospital_ros_run.py @@ -42,8 +42,9 @@ def main(): ros2 = ROS2_conn() NavigationSystemProcess = NavigationSystem.init() ros_control = RosControlPlugin(scan_interval=0.1) - ros_control.create_topic_server(ClawProcessor.RosClawService(event_store=eventStore, world=world)) - ros_control.create_topic_server(ClawProcessor.RosClawDropService(event_store=eventStore, world=world)) + claw_services = ClawProcessor.create_grab_and_drop_for_each_robot(world=world, event_store=eventStore) + for service in claw_services: + ros_control.create_topic_server(service) nav2_services = Nav2System.create_services(event_store=eventStore, world=world) for service in nav2_services: ros_control.create_action_server(service) diff --git a/src/simulator/systems/ClawDESProcessor.py b/src/simulator/systems/ClawDESProcessor.py index e9bac89..8ef3d0a 100644 --- a/src/simulator/systems/ClawDESProcessor.py +++ b/src/simulator/systems/ClawDESProcessor.py @@ -158,24 +158,31 @@ def dropInstrution(ent: int, args: List[str], script: Script, event_store: Filte script.expecting.append(ClawDoneTag) return script.state +def create_grab_and_drop_for_each_robot(world, event_store): + services = [] + for ent, (vel, pos, ros_goal) in world.get_components(Velocity, Position, NavToPoseRosGoal): + grab = RosClawGrabService(event_store=event_store, world=world, robot_name=ros_goal.name) + drop = RosClawDropService(event_store=event_store, world=world, robot_name=ros_goal.name) + services.append(grab) + services.append(drop) + return services -class RosClawService(RosTopicServer): +class RosClawGrabService(RosTopicServer): def __init__(self, **kwargs): super().__init__() self.logger = logging.getLogger(__name__) self.event_store = kwargs.get('event_store', None) self.world = kwargs.get('world', None) + self.robot_name = kwargs.get('robot_name', None) def get_name(self): - return "claw" + return self.robot_name + "/grab" def listener_callback(self, msg): - robot, object = get_claw_msg_data(msg.data) - if robot is None: - return - self.logger.info(f'Received order for {robot} to grab') - entity = find_robot_in_world(self.world, robot) + object = msg.data + self.logger.info(f'Received order for {self.robot_name} to grab {object}') + entity = find_robot_in_world(self.world, self.robot_name) if entity is None: return payload = GRAB_ClawPayload(op=ClawOps.GRAB, obj=object, me=entity) @@ -192,16 +199,15 @@ def __init__(self, **kwargs): self.logger = logging.getLogger(__name__) self.event_store = kwargs.get('event_store', None) self.world = kwargs.get('world', None) + self.robot_name = kwargs.get('robot_name', None) def get_name(self): - return "drop" + return self.robot_name + "/drop" def listener_callback(self, msg): - robot, object = get_claw_msg_data(msg.data) - if robot is None: - return - self.logger.info(f'Received order for {robot} to drop') - entity = find_robot_in_world(self.world, robot) + object = msg.data + self.logger.info(f'Received order for {self.robot_name} to drop {object}') + entity = find_robot_in_world(self.world, self.robot_name) if entity is None: return payload = GRAB_ClawPayload(op=ClawOps.DROP, obj=object, me=entity) @@ -211,13 +217,6 @@ def listener_callback(self, msg): def get_listener_callback(self): return self.listener_callback -def get_claw_msg_data(msg): - msg = msg.split(' ') - if len(msg) != 2: - logging.getLogger(__name__).warn('Message not in the correct format: robot_name object') - return None, None - return msg[0], msg[1] - def find_robot_in_world(world, robot_name): for ent, (vel, pos, ros_goal) in world.get_components(Velocity, Position, NavToPoseRosGoal): if ros_goal.name == robot_name: