From 94da5d9870555bd7a153b75a3b3ce0852400ed0b Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Fri, 28 Feb 2025 14:48:03 +0100 Subject: [PATCH 01/10] feat: add monitoring if sim and robotic stack processes are working --- src/rai_sim/rai_sim/o3de/o3de_bridge.py | 51 +++++++++++++++++++++++++ 1 file changed, 51 insertions(+) diff --git a/src/rai_sim/rai_sim/o3de/o3de_bridge.py b/src/rai_sim/rai_sim/o3de/o3de_bridge.py index 924465879..cba930071 100644 --- a/src/rai_sim/rai_sim/o3de/o3de_bridge.py +++ b/src/rai_sim/rai_sim/o3de/o3de_bridge.py @@ -16,6 +16,7 @@ import shlex import signal import subprocess +import threading import time from pathlib import Path from typing import Any, Dict, List, Optional, Set @@ -41,6 +42,10 @@ ) +class ProcessMonitorError(Exception): + """Exception raised when a monitored process terminates unexpectedly.""" + + class O3DExROS2SimulationConfig(SimulationConfig): binary_path: Path level: Optional[str] = None @@ -69,6 +74,52 @@ def __init__( self.current_robotic_stack_process = None self.current_binary_path = None + self._monitor_exception = None + self._monitor_running = True + + self.monitor_thread = threading.Thread( + target=self._process_monitor, daemon=True + ) + self.monitor_thread.start() + + def _process_monitor(self): + """Background thread to check if the simulation and robotic stack processes are still running.""" + try: + while self._monitor_running: + time.sleep(2) # Check every 2 seconds + + if ( + self.current_sim_process + and self.current_sim_process.poll() is not None + ): + error_msg = "Simulation process has unexpectedly terminated!" + self.logger.error(error_msg) + raise ProcessMonitorError(error_msg) + + if ( + self.current_robotic_stack_process + and self.current_robotic_stack_process.poll() is not None + ): + error_msg = "Robotic stack process has unexpectedly terminated!" + self.logger.error(error_msg) + raise ProcessMonitorError(error_msg) + except Exception as e: + self._monitor_exception = e + self.logger.error(f"Process monitor detected an error: {str(e)}") + + def check_monitor_status(self): + """ + Checks if the process monitor has detected any errors. + This method should be called regularly by the main thread. + + Raises: + Exception: If the monitor thread has detected an error + """ + if self._monitor_exception: + exception = self._monitor_exception + self._monitor_exception = None + raise exception + def shutdown(self): self._shutdown_binary() self._shutdown_robotic_stack() From 647df53bc693ba6babc3900933dce38a42c21ff5 Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Mon, 24 Mar 2025 11:31:56 +0100 Subject: [PATCH 02/10] refactor: extracted functionality related to monitoring processes to separate class, modified monitor_process method by sending sigint to main process --- src/rai_sim/rai_sim/o3de/o3de_bridge.py | 96 +++++++++++++------------ 1 file changed, 50 insertions(+), 46 deletions(-) diff --git a/src/rai_sim/rai_sim/o3de/o3de_bridge.py b/src/rai_sim/rai_sim/o3de/o3de_bridge.py index cba930071..09aac514a 100644 --- a/src/rai_sim/rai_sim/o3de/o3de_bridge.py +++ b/src/rai_sim/rai_sim/o3de/o3de_bridge.py @@ -13,6 +13,7 @@ # limitations under the License. import logging +import os import shlex import signal import subprocess @@ -74,52 +75,6 @@ def __init__( self.current_robotic_stack_process = None self.current_binary_path = None - self._monitor_exception = None - self._monitor_running = True - - self.monitor_thread = threading.Thread( - target=self._process_monitor, daemon=True - ) - self.monitor_thread.start() - - def _process_monitor(self): - """Background thread to check if the simulation and robotic stack processes are still running.""" - try: - while self._monitor_running: - time.sleep(2) # Check every 2 seconds - - if ( - self.current_sim_process - and self.current_sim_process.poll() is not None - ): - error_msg = "Simulation process has unexpectedly terminated!" - self.logger.error(error_msg) - raise ProcessMonitorError(error_msg) - - if ( - self.current_robotic_stack_process - and self.current_robotic_stack_process.poll() is not None - ): - error_msg = "Robotic stack process has unexpectedly terminated!" - self.logger.error(error_msg) - raise ProcessMonitorError(error_msg) - except Exception as e: - self._monitor_exception = e - self.logger.error(f"Process monitor detected an error: {str(e)}") - - def check_monitor_status(self): - """ - Checks if the process monitor has detected any errors. - This method should be called regularly by the main thread. - - Raises: - Exception: If the monitor thread has detected an error - """ - if self._monitor_exception: - exception = self._monitor_exception - self._monitor_exception = None - raise exception - def shutdown(self): self._shutdown_binary() self._shutdown_robotic_stack() @@ -500,3 +455,52 @@ def move_arm( result = get_future_result(future, timeout_sec=5.0) self.connector.node.get_logger().debug(f"Moving arm result: {result}") + + +class O3DExROS2BridgeProcessesMonitor: + def __init__( + self, bridge: O3DExROS2Bridge, logger: Optional[logging.Logger] = None + ): + self.bridge = bridge + if logger is None: + self.logger = logging.getLogger(__name__) + else: + self.logger = logger + self.is_running = True + + self.monitor_thread = threading.Thread( + target=self._monitor_processes, daemon=True + ) + self.monitor_thread.start() + self.logger.info("O3DExROS2Bridge processes monitor started") + + def _monitor_processes(self): + """Background thread that monitors the simulation and robotic stack processes""" + while self.is_running: + if ( + self.bridge.current_sim_process + and self.bridge.current_sim_process.poll() is not None + ): + self.logger.error( + f"Simulation process exited unexpectedly with code {self.bridge.current_sim_process.poll()}" + ) + self.logger.info("Sending SIGINT to main process...") + os.kill(os.getpid(), signal.SIGINT) + + if ( + self.bridge.current_robotic_stack_process + and self.bridge.current_robotic_stack_process.poll() is not None + ): + self.logger.error( + f"Robotic stack process exited unexpectedly with code {self.bridge.current_robotic_stack_process.poll()}" + ) + self.logger.info("Sending SIGINT to main process...") + os.kill(os.getpid(), signal.SIGINT) + + time.sleep(1) + + def shutdown(self): + self.is_running = False + if self.monitor_thread.is_alive(): + self.monitor_thread.join(timeout=2) + self.logger.info("Processes monitor thread shut down.") From 5f5428205890cfd966df4ca18e3288cd1269d47d Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Mon, 24 Mar 2025 12:29:55 +0100 Subject: [PATCH 03/10] chore: removed logging info left after debugging --- src/rai_sim/rai_sim/o3de/o3de_bridge.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/rai_sim/rai_sim/o3de/o3de_bridge.py b/src/rai_sim/rai_sim/o3de/o3de_bridge.py index 09aac514a..a80c090f5 100644 --- a/src/rai_sim/rai_sim/o3de/o3de_bridge.py +++ b/src/rai_sim/rai_sim/o3de/o3de_bridge.py @@ -287,7 +287,6 @@ def setup_scene( else: while self.spawned_entities: self._despawn_entity(self.spawned_entities[0]) - self.logger.info(f"Entities after despawn: {self.spawned_entities}") for entity in simulation_config.entities: self._spawn_entity(entity) From f9ed252e4e87810722a0d09e6ce9d28b316cd9c4 Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Mon, 24 Mar 2025 15:05:42 +0100 Subject: [PATCH 04/10] chore: remove unused ProcessMonitorError Exception --- src/rai_sim/rai_sim/o3de/o3de_bridge.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/rai_sim/rai_sim/o3de/o3de_bridge.py b/src/rai_sim/rai_sim/o3de/o3de_bridge.py index a80c090f5..5f22fcf2c 100644 --- a/src/rai_sim/rai_sim/o3de/o3de_bridge.py +++ b/src/rai_sim/rai_sim/o3de/o3de_bridge.py @@ -43,10 +43,6 @@ ) -class ProcessMonitorError(Exception): - """Exception raised when a monitored process terminates unexpectedly.""" - - class O3DExROS2SimulationConfig(SimulationConfig): binary_path: Path level: Optional[str] = None From e93c6b6d16907b47791e4467f4bcd082fc6a7a38 Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Tue, 25 Mar 2025 12:17:30 +0100 Subject: [PATCH 05/10] refactor: moved processes monitoring functionalities to SimulationBridge class --- src/rai_sim/rai_sim/o3de/o3de_bridge.py | 76 ++++++++---------------- src/rai_sim/rai_sim/simulation_bridge.py | 44 ++++++++++++-- 2 files changed, 64 insertions(+), 56 deletions(-) diff --git a/src/rai_sim/rai_sim/o3de/o3de_bridge.py b/src/rai_sim/rai_sim/o3de/o3de_bridge.py index 5f22fcf2c..0942915d1 100644 --- a/src/rai_sim/rai_sim/o3de/o3de_bridge.py +++ b/src/rai_sim/rai_sim/o3de/o3de_bridge.py @@ -13,15 +13,14 @@ # limitations under the License. import logging -import os import shlex import signal import subprocess -import threading import time from pathlib import Path from typing import Any, Dict, List, Optional, Set +import psutil import yaml from geometry_msgs.msg import Point, PoseStamped, Quaternion from geometry_msgs.msg import Pose as ROS2Pose @@ -34,6 +33,7 @@ from rai_sim.simulation_bridge import ( Entity, Pose, + Process, Rotation, SceneState, SimulationBridge, @@ -78,6 +78,10 @@ def shutdown(self): def _shutdown_binary(self): if not self.current_sim_process: return + process_to_remove = [ + p for p in self._processes if p.process == self.current_sim_process + ][0] + self._processes.remove(process_to_remove) self.current_sim_process.send_signal(signal.SIGINT) self.current_sim_process.wait() @@ -94,7 +98,12 @@ def _shutdown_binary(self): def _shutdown_robotic_stack(self): if not self.current_robotic_stack_process: return - + process_to_remove = [ + p + for p in self._processes + if p.process == self.current_robotic_stack_process + ][0] + self._processes.remove(process_to_remove) self.current_robotic_stack_process.send_signal(signal.SIGINT) self.current_robotic_stack_process.wait() @@ -306,6 +315,12 @@ def _launch_binary( required_ros2_stack=simulation_config.required_simulation_ros2_interfaces ): raise RuntimeError("ROS2 stack is not ready in time.") + self._processes.append( + Process( + name=psutil.Process(self.current_sim_process.pid).name(), + process=self.current_sim_process, + ) + ) def _launch_robotic_stack(self, simulation_config: O3DExROS2SimulationConfig): command = shlex.split(simulation_config.robotic_stack_command) @@ -319,6 +334,12 @@ def _launch_robotic_stack(self, simulation_config: O3DExROS2SimulationConfig): required_ros2_stack=simulation_config.required_robotic_ros2_interfaces ): raise RuntimeError("ROS2 stack is not ready in time.") + self._processes.append( + Process( + name=psutil.Process(self.current_robotic_stack_process.pid).name(), + process=self.current_robotic_stack_process, + ) + ) def _has_process_started(self, process: subprocess.Popen[Any], timeout: int = 15): start_time = time.time() @@ -450,52 +471,3 @@ def move_arm( result = get_future_result(future, timeout_sec=5.0) self.connector.node.get_logger().debug(f"Moving arm result: {result}") - - -class O3DExROS2BridgeProcessesMonitor: - def __init__( - self, bridge: O3DExROS2Bridge, logger: Optional[logging.Logger] = None - ): - self.bridge = bridge - if logger is None: - self.logger = logging.getLogger(__name__) - else: - self.logger = logger - self.is_running = True - - self.monitor_thread = threading.Thread( - target=self._monitor_processes, daemon=True - ) - self.monitor_thread.start() - self.logger.info("O3DExROS2Bridge processes monitor started") - - def _monitor_processes(self): - """Background thread that monitors the simulation and robotic stack processes""" - while self.is_running: - if ( - self.bridge.current_sim_process - and self.bridge.current_sim_process.poll() is not None - ): - self.logger.error( - f"Simulation process exited unexpectedly with code {self.bridge.current_sim_process.poll()}" - ) - self.logger.info("Sending SIGINT to main process...") - os.kill(os.getpid(), signal.SIGINT) - - if ( - self.bridge.current_robotic_stack_process - and self.bridge.current_robotic_stack_process.poll() is not None - ): - self.logger.error( - f"Robotic stack process exited unexpectedly with code {self.bridge.current_robotic_stack_process.poll()}" - ) - self.logger.info("Sending SIGINT to main process...") - os.kill(os.getpid(), signal.SIGINT) - - time.sleep(1) - - def shutdown(self): - self.is_running = False - if self.monitor_thread.is_alive(): - self.monitor_thread.join(timeout=2) - self.logger.info("Processes monitor thread shut down.") diff --git a/src/rai_sim/rai_sim/simulation_bridge.py b/src/rai_sim/rai_sim/simulation_bridge.py index 9241583e4..f6d04dc73 100644 --- a/src/rai_sim/rai_sim/simulation_bridge.py +++ b/src/rai_sim/rai_sim/simulation_bridge.py @@ -13,9 +13,15 @@ # limitations under the License. import logging +import os +import signal +import subprocess +import threading +import time from abc import ABC, abstractmethod +from dataclasses import dataclass from pathlib import Path -from typing import Generic, List, Optional, TypeVar +from typing import Any, Generic, List, Optional, TypeVar import yaml from pydantic import BaseModel, Field, field_validator @@ -163,6 +169,12 @@ class SceneState(BaseModel): ) +@dataclass(frozen=True) +class Process: + name: str + process: subprocess.Popen[Any] + + SimulationConfigT = TypeVar("SimulationConfigT", bound=SimulationConfig) @@ -172,14 +184,20 @@ class SimulationBridge(ABC, Generic[SimulationConfigT]): """ def __init__(self, logger: Optional[logging.Logger] = None): - self.spawned_entities: List[ - SpawnedEntity - ] = [] # list of spawned entities with their initial poses + self.spawned_entities: List[SpawnedEntity] = [] + self._processes: List[Process] = [] + if logger is None: self.logger = logging.getLogger(__name__) else: self.logger = logger + self._monitoring_running = True + self._process_monitor_thread = threading.Thread( + target=self._monitor_processes, daemon=True + ) + self._process_monitor_thread.start() + @abstractmethod def setup_scene(self, simulation_config: SimulationConfigT): """ @@ -279,3 +297,21 @@ def get_scene_state(self) -> SceneState: SceneState should contain the current poses of spawned_entities. """ pass + + def _monitor_processes(self): + """Checks the status of managed processes and shuts everything down if one of the processes exits unexpectedly.""" + while self._monitoring_running: + for process in self._processes[:]: + if process.process.poll() is not None: + self.logger.error( + f"Process {process.name} with PID {process.process.pid} exited unexpectedly with code {process.process.returncode}" + ) + self.logger.info("Shutting down all processes.") + os.kill(os.getpid(), signal.SIGINT) + time.sleep(1) + + def stop_monitoring(self): + self._monitoring_running = False + if self._process_monitor_thread.is_alive(): + self._process_monitor_thread.join() + self.logger.info("Processes monitor thread shut down.") From 12cc17f3d73971faadd108d32b0f987afb4b5540 Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Tue, 25 Mar 2025 21:20:03 +0100 Subject: [PATCH 06/10] feat: monitor child processes of robotic stack to monitor ros2 nodes --- src/rai_sim/rai_sim/o3de/o3de_bridge.py | 23 ++++++++++++---------- src/rai_sim/rai_sim/simulation_bridge.py | 25 ++++++++++++++++-------- 2 files changed, 30 insertions(+), 18 deletions(-) diff --git a/src/rai_sim/rai_sim/o3de/o3de_bridge.py b/src/rai_sim/rai_sim/o3de/o3de_bridge.py index 0942915d1..b338e728b 100644 --- a/src/rai_sim/rai_sim/o3de/o3de_bridge.py +++ b/src/rai_sim/rai_sim/o3de/o3de_bridge.py @@ -74,14 +74,11 @@ def __init__( def shutdown(self): self._shutdown_binary() self._shutdown_robotic_stack() + self._processes = [] def _shutdown_binary(self): if not self.current_sim_process: return - process_to_remove = [ - p for p in self._processes if p.process == self.current_sim_process - ][0] - self._processes.remove(process_to_remove) self.current_sim_process.send_signal(signal.SIGINT) self.current_sim_process.wait() @@ -98,12 +95,6 @@ def _shutdown_binary(self): def _shutdown_robotic_stack(self): if not self.current_robotic_stack_process: return - process_to_remove = [ - p - for p in self._processes - if p.process == self.current_robotic_stack_process - ][0] - self._processes.remove(process_to_remove) self.current_robotic_stack_process.send_signal(signal.SIGINT) self.current_robotic_stack_process.wait() @@ -315,6 +306,7 @@ def _launch_binary( required_ros2_stack=simulation_config.required_simulation_ros2_interfaces ): raise RuntimeError("ROS2 stack is not ready in time.") + self._processes.append( Process( name=psutil.Process(self.current_sim_process.pid).name(), @@ -334,6 +326,7 @@ def _launch_robotic_stack(self, simulation_config: O3DExROS2SimulationConfig): required_ros2_stack=simulation_config.required_robotic_ros2_interfaces ): raise RuntimeError("ROS2 stack is not ready in time.") + self._processes.append( Process( name=psutil.Process(self.current_robotic_stack_process.pid).name(), @@ -341,6 +334,16 @@ def _launch_robotic_stack(self, simulation_config: O3DExROS2SimulationConfig): ) ) + parent = psutil.Process(self.current_robotic_stack_process.pid) + children = parent.children(recursive=True) + for child in children: + self._processes.append( + Process( + name=psutil.Process(child.pid).name(), + process=child, + ) + ) + def _has_process_started(self, process: subprocess.Popen[Any], timeout: int = 15): start_time = time.time() while time.time() - start_time < timeout: diff --git a/src/rai_sim/rai_sim/simulation_bridge.py b/src/rai_sim/rai_sim/simulation_bridge.py index f6d04dc73..ed0adbae4 100644 --- a/src/rai_sim/rai_sim/simulation_bridge.py +++ b/src/rai_sim/rai_sim/simulation_bridge.py @@ -21,9 +21,10 @@ from abc import ABC, abstractmethod from dataclasses import dataclass from pathlib import Path -from typing import Any, Generic, List, Optional, TypeVar +from typing import Any, Generic, List, Optional, TypeVar, Union import yaml +from psutil import Process as PsutilProcess from pydantic import BaseModel, Field, field_validator @@ -172,7 +173,7 @@ class SceneState(BaseModel): @dataclass(frozen=True) class Process: name: str - process: subprocess.Popen[Any] + process: Union[subprocess.Popen[Any], PsutilProcess] SimulationConfigT = TypeVar("SimulationConfigT", bound=SimulationConfig) @@ -302,12 +303,20 @@ def _monitor_processes(self): """Checks the status of managed processes and shuts everything down if one of the processes exits unexpectedly.""" while self._monitoring_running: for process in self._processes[:]: - if process.process.poll() is not None: - self.logger.error( - f"Process {process.name} with PID {process.process.pid} exited unexpectedly with code {process.process.returncode}" - ) - self.logger.info("Shutting down all processes.") - os.kill(os.getpid(), signal.SIGINT) + if isinstance(process.process, subprocess.Popen): + if process.process.poll() is not None: + self.logger.critical( + f"Process {process.name} with PID {process.process.pid} exited unexpectedly with code {process.process.returncode}" + ) + self.logger.info("Shutting down main process.") + os.kill(os.getpid(), signal.SIGINT) + else: + if not process.process.is_running(): + self.logger.critical( + f"Process {process.name} with PID {process.process.pid} exited unexpectedly." + ) + self.logger.info("Shutting down main process.") + os.kill(os.getpid(), signal.SIGINT) time.sleep(1) def stop_monitoring(self): From f091ca1a547bef1b22d540e2fb64637eb6dda244 Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Tue, 25 Mar 2025 21:36:43 +0100 Subject: [PATCH 07/10] fix: removed converting child process to psutil.Process because it already is psutil.Process --- src/rai_sim/rai_sim/o3de/o3de_bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rai_sim/rai_sim/o3de/o3de_bridge.py b/src/rai_sim/rai_sim/o3de/o3de_bridge.py index b338e728b..920a76ff7 100644 --- a/src/rai_sim/rai_sim/o3de/o3de_bridge.py +++ b/src/rai_sim/rai_sim/o3de/o3de_bridge.py @@ -339,7 +339,7 @@ def _launch_robotic_stack(self, simulation_config: O3DExROS2SimulationConfig): for child in children: self._processes.append( Process( - name=psutil.Process(child.pid).name(), + name=child.name(), process=child, ) ) From 3e1ebd4a635cb745aa908d5f7f09a19d11f89696 Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Tue, 25 Mar 2025 21:38:13 +0100 Subject: [PATCH 08/10] refactor: psutil.Process import refactor --- src/rai_sim/rai_sim/simulation_bridge.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rai_sim/rai_sim/simulation_bridge.py b/src/rai_sim/rai_sim/simulation_bridge.py index ed0adbae4..fcba550d7 100644 --- a/src/rai_sim/rai_sim/simulation_bridge.py +++ b/src/rai_sim/rai_sim/simulation_bridge.py @@ -23,8 +23,8 @@ from pathlib import Path from typing import Any, Generic, List, Optional, TypeVar, Union +import psutil import yaml -from psutil import Process as PsutilProcess from pydantic import BaseModel, Field, field_validator @@ -173,7 +173,7 @@ class SceneState(BaseModel): @dataclass(frozen=True) class Process: name: str - process: Union[subprocess.Popen[Any], PsutilProcess] + process: Union[subprocess.Popen[Any], psutil.Process] SimulationConfigT = TypeVar("SimulationConfigT", bound=SimulationConfig) From 09481fb127b23d8a678c3ae063fb8005babc3ebd Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Tue, 25 Mar 2025 22:46:12 +0100 Subject: [PATCH 09/10] test: adjust launch_binary and launch_robotic_stack tests --- tests/rai_sim/test_o3de_bridge.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/tests/rai_sim/test_o3de_bridge.py b/tests/rai_sim/test_o3de_bridge.py index b7687304a..137bc6029 100644 --- a/tests/rai_sim/test_o3de_bridge.py +++ b/tests/rai_sim/test_o3de_bridge.py @@ -118,18 +118,24 @@ def test_init(self): self.assertEqual(self.bridge.spawned_entities, []) @patch("subprocess.Popen") - def test_launch_robotic_stack(self, mock_popen): + @patch("psutil.Process") + def test_launch_robotic_stack( + self, mock_psutil_process: MagicMock, mock_popen: MagicMock + ): mock_process = MagicMock() mock_process.poll.return_value = None mock_process.pid = 54321 mock_popen.return_value = mock_process + self.bridge._launch_robotic_stack(self.test_config) mock_popen.assert_called_once_with(["ros2", "launch", "robot.launch.py"]) + mock_psutil_process.assert_any_call(mock_process.pid) self.assertEqual(self.bridge.current_robotic_stack_process, mock_process) @patch("subprocess.Popen") - def test_launch_binary(self, mock_popen): + @patch("psutil.Process") + def test_launch_binary(self, mock_psutil_process: MagicMock, mock_popen: MagicMock): mock_process = MagicMock() mock_process.poll.return_value = None mock_process.pid = 54322 @@ -138,6 +144,7 @@ def test_launch_binary(self, mock_popen): self.bridge._launch_binary(self.test_config) mock_popen.assert_called_once_with(["/path/to/binary"]) + mock_psutil_process.assert_called_once_with(mock_process.pid) self.assertEqual(self.bridge.current_sim_process, mock_process) def test_shutdown_binary(self): From 39e7541db800b7d03c79b8a51ef146be85860eba Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Thu, 27 Mar 2025 10:32:36 +0100 Subject: [PATCH 10/10] fix: adjusted scene setup after adding level to the binary Related to the change in bbc6bc2b5e0e43f2aff1e845fda86cef9ec1e632. --- src/rai_sim/rai_sim/o3de/o3de_bridge.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/rai_sim/rai_sim/o3de/o3de_bridge.py b/src/rai_sim/rai_sim/o3de/o3de_bridge.py index 920a76ff7..8e6de7658 100644 --- a/src/rai_sim/rai_sim/o3de/o3de_bridge.py +++ b/src/rai_sim/rai_sim/o3de/o3de_bridge.py @@ -70,6 +70,7 @@ def __init__( self.current_sim_process = None self.current_robotic_stack_process = None self.current_binary_path = None + self.current_binary_level = None def shutdown(self): self._shutdown_binary() @@ -273,12 +274,16 @@ def setup_scene( self, simulation_config: O3DExROS2SimulationConfig, ): - if self.current_binary_path != simulation_config.binary_path: + if ( + self.current_binary_path != simulation_config.binary_path + or self.current_binary_level != simulation_config.level + ): if self.current_sim_process: self.shutdown() self._launch_binary(simulation_config) self._launch_robotic_stack(simulation_config) self.current_binary_path = simulation_config.binary_path + self.current_binary_level = simulation_config.level else: while self.spawned_entities: