feat: simulation and benchmark suite

.gitignore

@@ -172,3 +172,5 @@ logs/
 
 src/examples/*-demo
 artifact_database.pkl
+
+imgui.ini

examples/manipulation-demo-no-binary.launch.py

@@ -0,0 +1,59 @@
+# Copyright (C) 2025 Robotec.AI
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#         http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import (
+    IncludeLaunchDescription,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+# TODO (mkotynia) think about separation of launches
+def generate_launch_description():
+    launch_moveit = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                "src/examples/rai-manipulation-demo/Project/Examples/panda_moveit_config_demo.launch.py",
+            ]
+        )
+    )
+
+    launch_robotic_manipulation = Node(
+        package="robotic_manipulation",
+        executable="robotic_manipulation",
+        # name="robotic_manipulation_node",
+        output="screen",
+        parameters=[
+            {"use_sim_time": True},
+        ],
+    )
+
+    launch_openset = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                FindPackageShare("rai_bringup"),
+                "/launch/openset.launch.py",
+            ]
+        ),
+    )
+
+    return LaunchDescription(
+        [
+            launch_openset,
+            launch_moveit,
+            launch_robotic_manipulation,
+        ]
+    )

examples/manipulation-demo.launch.py

@@ -12,22 +12,16 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import rclpy
-from launch import LaunchContext, LaunchDescription
+from launch import LaunchDescription
 from launch.actions import (
     DeclareLaunchArgument,
     ExecuteProcess,
     IncludeLaunchDescription,
-    OpaqueFunction,
-    RegisterEventHandler,
 )
-from launch.event_handlers import OnExecutionComplete, OnProcessStart
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
-from rclpy.qos import QoSProfile, ReliabilityPolicy
-from rosgraph_msgs.msg import Clock
 
 
 def generate_launch_description():
@@ -46,21 +40,6 @@ def generate_launch_description():
         output="screen",
     )
 
-    def wait_for_clock_message(context: LaunchContext, *args, **kwargs):
-        rclpy.init()
-        node = rclpy.create_node("wait_for_game_launcher")
-        node.create_subscription(
-            Clock,
-            "/clock",
-            lambda msg: rclpy.shutdown(),
-            QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
-        )
-        rclpy.spin(node)
-        return None
-
-    # Game launcher will start publishing the clock message after loading the simulation
-    wait_for_game_launcher = OpaqueFunction(function=wait_for_clock_message)
-
     launch_moveit = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             [
@@ -72,7 +51,6 @@ def wait_for_clock_message(context: LaunchContext, *args, **kwargs):
     launch_robotic_manipulation = Node(
         package="robotic_manipulation",
         executable="robotic_manipulation",
-        name="robotic_manipulation_node",
         output="screen",
         parameters=[
             {"use_sim_time": True},
@@ -90,28 +68,10 @@ def wait_for_clock_message(context: LaunchContext, *args, **kwargs):
 
     return LaunchDescription(
         [
-            # Include the game_launcher argument
             game_launcher_arg,
-            # Launch the game launcher and wait for it to load
             launch_game_launcher,
-            RegisterEventHandler(
-                event_handler=OnProcessStart(
-                    target_action=launch_game_launcher,
-                    on_start=[
-                        wait_for_game_launcher,
-                    ],
-                )
-            ),
-            # Launch the MoveIt node after loading the simulation
-            RegisterEventHandler(
-                event_handler=OnExecutionComplete(
-                    target_action=wait_for_game_launcher,
-                    on_completion=[
-                        launch_openset,
-                        launch_moveit,
-                        launch_robotic_manipulation,
-                    ],
-                )
-            ),
+            launch_openset,
+            launch_moveit,
+            launch_robotic_manipulation,
         ]
     )

examples/manipulation-demo.py

@@ -12,37 +12,37 @@
 # See the License for the specific language goveself.rning permissions and
 # limitations under the License.
 
-import threading
 
 import rclpy
 import rclpy.qos
 from langchain_core.messages import HumanMessage
 from rai.agents.conversational_agent import create_conversational_agent
-from rai.node import RaiBaseNode
+from rai.communication.ros2.connectors import ROS2ARIConnector
 from rai.tools.ros.manipulation import GetObjectPositionsTool, MoveToPointTool
-from rai.tools.ros.native import GetCameraImage, Ros2GetTopicsNamesAndTypesTool
+from rai.tools.ros2.topics import GetROS2ImageTool, GetROS2TopicsNamesAndTypesTool
 from rai.utils.model_initialization import get_llm_model
+from rai_open_set_vision.tools import GetGrabbingPointTool
 
 
 def create_agent():
     rclpy.init()
-    node = RaiBaseNode(node_name="manipulation_demo")
+    connector = ROS2ARIConnector()
+    node = connector.node
     node.declare_parameter("conversion_ratio", 1.0)
 
-    threading.Thread(target=node.spin).start()
-
     tools = [
         GetObjectPositionsTool(
-            node=node,
+            connector=connector,
             target_frame="panda_link0",
             source_frame="RGBDCamera5",
             camera_topic="/color_image5",
             depth_topic="/depth_image5",
             camera_info_topic="/color_camera_info5",
+            get_grabbing_point_tool=GetGrabbingPointTool(connector=connector),
         ),
-        MoveToPointTool(node=node, manipulator_frame="panda_link0"),
-        GetCameraImage(node=node),
-        Ros2GetTopicsNamesAndTypesTool(node=node),
+        MoveToPointTool(connector=connector, manipulator_frame="panda_link0"),
+        GetROS2ImageTool(connector=connector),
+        GetROS2TopicsNamesAndTypesTool(connector=connector),
     ]
 
     llm = get_llm_model(model_type="complex_model", streaming=True)

pyproject.toml

@@ -17,6 +17,8 @@ python = "^3.10, <3.13"
 rai = {path = "src/rai_core", develop = true}
 rai_asr = {path = "src/rai_asr", develop = true}
 rai_tts = {path = "src/rai_tts", develop = true}
+rai_sim = {path = "src/rai_sim", develop = true}
+rai_bench = {path = "src/rai_bench", develop = true}
 
 langchain-core = "^0.3"
 langchain = "*"

setup_shell.sh

@@ -30,3 +30,8 @@ esac
 
 export PYTHONPATH
 PYTHONPATH="$(dirname "$(dirname "$(poetry run which python)")")/lib/python$(poetry run python --version | awk '{print $2}' | cut -d. -f1,2)/site-packages:$PYTHONPATH"
+PYTHONPATH="src/rai_core:$PYTHONPATH"
+PYTHONPATH="src/rai_asr:$PYTHONPATH"
+PYTHONPATH="src/rai_tts:$PYTHONPATH"
+PYTHONPATH="src/rai_sim:$PYTHONPATH"
+PYTHONPATH="src/rai_bench:$PYTHONPATH"

src/rai_bench/README.md

@@ -0,0 +1,54 @@
+## RAI Benchmark
+
+## Description
+
+The RAI Bench is a package including benchmarks and providing frame for creating new benchmarks
+
+## Frame Components
+
+Frame components can be found in `src/rai_bench/rai_bench/benchmark_model.py`
+
+- `Task`
+- `Scenario`
+- `Benchmark`
+
+For more information about these classes go to -> `src/rai_bench/rai_bench/benchmark_model.py`
+
+### O3DE TEST BENCHMARK
+
+O3DE Test Benchmark (`src/rai_bench/rai_bench/o3de_test_bench/`), contains 2 Tasks(`tasks/`) - GrabCarrotTask and PlaceCubesTask (these tasks implement calculating scores) and 4 scene_configs(`configs/`) for O3DE robotic arm simulation.
+
+Both tasks calculate score, taking into consideration 4 values:
+
+- initially_misplaced_now_correct
+- initially_misplaced_still_incorrect
+- initially_correct_still_correct
+- initially_correct_now_incorrect
+
+The result is a value between 0 and 1, calculated like (initially_misplaced_now_correct + initially_correct_still_correct) / number_of_initial_objects.
+This score is calculated at the beggining and at the end of each scenario.
+
+### Example usage
+
+Example of how to load scenes, define scenarios and run benchmark can be found in `src/rai_bench/rai_bench/examples/o3de_test_benchmark.py`
+
+Scenarios can be loaded manually like:
+
+```python
+one_carrot_simulation_config = O3DExROS2SimulationConfig.load_config(
+        base_config_path=Path("path_to_scene.yaml"),
+        connector_config_path=Path("path_to_o3de_config.yaml"),
+    )
+
+Scenario(task=GrabCarrotTask(logger=some_logger), simulation_config=one_carrot_simulation_config)
+```
+
+or automatically like:
+
+```python
+scenarios = Benchmark.create_scenarios(
+        tasks=tasks, simulation_configs=simulations_configs
+    )
+```
+
+which will result in list of scenarios with combination of every possible task and scene(task decides if scene config is suitable for it).

src/rai_bench/pyproject.toml

@@ -0,0 +1,17 @@
+[tool.poetry]
+name = "rai-bench"
+version = "0.1.0"
+description = "Package for running and creating benchmarks."
+authors = ["jmatejcz <[email protected]>"]
+readme = "README.md"
+
+packages = [
+    { include = "rai_bench", from = "." },
+]
+[tool.poetry.dependencies]
+python = "^3.10"
+
+
+[build-system]
+requires = ["poetry-core"]
+build-backend = "poetry.core.masonry.api"

src/rai_bench/rai_bench/__init__.py

@@ -0,0 +1,13 @@
+# Copyright (C) 2025 Robotec.AI
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#         http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.