fix: simbench fixes

src/rai_bench/rai_bench/examples/o3de_test_benchmark.py

@@ -15,6 +15,7 @@
 ########### EXAMPLE USAGE ###########
 import logging
 import time
+from datetime import datetime
 from pathlib import Path
 from typing import List
 
@@ -69,7 +70,12 @@
         GetROS2TopicsNamesAndTypesTool(connector=connector),
     ]
     # define loggers
-    log_file = "src/rai_bench/rai_bench/benchmark.log"
+    now = datetime.now()
+    experiment_dir = (
+        f"src/rai_bench/rai_bench/experiments/{now.strftime('%Y-%m-%d_%H-%M-%S')}"
+    )
+    Path(experiment_dir).mkdir(parents=True, exist_ok=True)
+    log_file = f"{experiment_dir}/benchmark.log"
     file_handler = logging.FileHandler(log_file)
     file_handler.setLevel(logging.DEBUG)
 
@@ -159,11 +165,12 @@
 
     o3de = O3DEngineArmManipulationBridge(connector, logger=agent_logger)
     # define benchamrk
+    results_filename = f"{experiment_dir}/results.csv"
     benchmark = Benchmark(
         simulation_bridge=o3de,
         scenarios=scenarios,
         logger=bench_logger,
-        results_filename="src/rai_bench/rai_bench/results.csv",
+        results_filename=results_filename,
     )
     for i, s in enumerate(scenarios):
         agent = create_conversational_agent(

src/rai_sim/rai_sim/o3de/o3de_bridge.py

@@ -176,6 +176,9 @@ def get_object_pose(self, entity: SpawnedEntity) -> Pose:
             ROS2Pose(),
             self.connector.get_transform(object_frame + "odom", object_frame),
         )
+        ros2_pose = do_transform_pose(
+            ros2_pose, self.connector.get_transform("world", "odom")
+        )
         return self._from_ros2_pose(ros2_pose)
 
     def get_scene_state(self) -> SceneState:
@@ -274,6 +277,7 @@ def setup_scene(self, simulation_config: O3DExROS2SimulationConfig):
         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)