From d0670ae5c8e0bd3fb3ad01a22bf633621c166308 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Thu, 2 Jul 2026 17:35:41 +0200 Subject: [PATCH 01/11] Populate scene using batch spawning MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- rai_app/environment/scene_manager.py | 96 +++++++++++++++++++++------- 1 file changed, 74 insertions(+), 22 deletions(-) diff --git a/rai_app/environment/scene_manager.py b/rai_app/environment/scene_manager.py index e7c5dda..051f02c 100644 --- a/rai_app/environment/scene_manager.py +++ b/rai_app/environment/scene_manager.py @@ -33,11 +33,14 @@ ) from rosidl_runtime_py.convert import message_to_ordereddict from simulation_interfaces.msg import EntityState +from simulation_interfaces.msg import SpawnEntity as SpawnEntityMsg from simulation_interfaces.srv import ( GetEntities, GetEntityState, SetEntityState, + SpawnEntities, SpawnEntity, + SpawnEntity_Request, ) from tf2_geometry_msgs import do_transform_pose from tf_transformations import euler_from_quaternion, quaternion_from_euler @@ -192,12 +195,8 @@ def populate_scene( "Slots and object names must have the same length and items stored" ) - simulation_names: list[str] = [] - for slot, object_name, item in tqdm( - zip(slots, object_names, items_stored), - desc="Spawning entities", - total=len(slots), - ): + reqs: list[SpawnEntityMsg] = [] + for slot, object_name, item in zip(slots, object_names, items_stored): if np.isclose(offset_yaw, 0.0): should_rotate = random.random() < percent_of_rotated_objects if should_rotate: @@ -206,7 +205,7 @@ def populate_scene( else: offset_yaw = 0.0 - simulation_name = self.spawn_on_spot( + req = self.make_spawn_entity_msg_for_spot( slot_name=slot, object_name=object_name, item_stored=item, @@ -214,18 +213,19 @@ def populate_scene( std_yaw=std_yaw, offset_yaw=offset_yaw, ) - self.logger.info(f"Simulation name: {simulation_name}") - simulation_names.append(simulation_name) + reqs.append(req) + + simulation_names: list[str] = self.spawn_objects(reqs) return simulation_names - def spawn_object( + def init_spawn_entity_name_and_pose( self, + req: SpawnEntityMsg | SpawnEntity_Request, pose: Pose, object_name: str, item_stored: Optional[str] = None, - frame: str = "odom", + frame: str = "odom" ): - wait_for_ros2_services(self.connector, ["/spawn_entity"]) # NOTE (jmatejcz) item stored will be added to name of object # and that's how it will be distinguished if item_stored: @@ -233,9 +233,7 @@ def spawn_object( else: name = object_name + str(uuid.uuid4())[:8] - req = SpawnEntity.Request() req.name = name - req.uri = self.spawnable_to_uri[object_name] req.initial_pose.header.frame_id = frame req.initial_pose.pose.position.x = pose.position.x req.initial_pose.pose.position.y = pose.position.y @@ -245,7 +243,41 @@ def spawn_object( req.initial_pose.pose.orientation.z = pose.orientation.z req.initial_pose.pose.orientation.w = pose.orientation.w - self.logger.debug(f"Spawning {name}") + return req + + + def make_spawn_entity_msg( + self, + pose: Pose, + object_name: str, + item_stored: Optional[str] = None, + frame: str = "odom" + ) -> SpawnEntityMsg: + req = SpawnEntityMsg() + self.init_spawn_entity_name_and_pose( + req, + pose, + object_name, + item_stored, + frame + ) + req.entity_resource.uri = self.spawnable_to_uri[object_name] + return req + + def spawn_object( + self, + pose: Pose, + object_name: str, + item_stored: Optional[str] = None, + frame: str = "odom", + ): + wait_for_ros2_services(self.connector, ["/spawn_entity"]) + + req = SpawnEntity.Request() + self.init_spawn_entity_name_and_pose(req, pose, object_name, item_stored, frame) + req.uri = self.spawnable_to_uri[object_name] + + self.logger.debug(f"Spawning {req.name}") result = self.connector.call_service( ROS2Message(payload=message_to_ordereddict(req)), target="/spawn_entity", @@ -254,9 +286,32 @@ def spawn_object( reuse_client=True, ).payload result = cast(SpawnEntity.Response, result) - return name + return req.name + + def spawn_objects( + self, + requests: list[SpawnEntityMsg] + ): + wait_for_ros2_services(self.connector, ["/spawn_entities"]) - def spawn_on_spot( + req = SpawnEntities.Request() + req.spawn_requests = requests + + self.logger.debug(f"Spawning {[request.name for request in requests]}") + result = self.connector.call_service( + ROS2Message(payload=message_to_ordereddict(req)), + target="/spawn_entities", + msg_type="simulation_interfaces/srv/SpawnEntities", + timeout_sec=3.0, + reuse_client=True, + ).payload + result = cast(SpawnEntities.Response, result) + for res in result.results: + if res.result.result != 1: + print(f"ERROR: {res.result.error_message}") + return [res.entity_name for res in result.results] + + def make_spawn_entity_msg_for_spot( self, slot_name: str, object_name: str, @@ -265,8 +320,7 @@ def spawn_on_spot( std_yaw: float = 0.0, offset_yaw: float = 0.0, frame: str = "odom", - ): - wait_for_ros2_services(self.connector, ["/spawn_entity"]) + ) -> SpawnEntityMsg: pose: Pose = copy.deepcopy(self.slots[slot_name].origin_pose) # Add Gaussian noise to x, y pose.position.x += random.normalvariate(0, std_xy) @@ -287,9 +341,7 @@ def spawn_on_spot( pose.orientation.z = q_new[2] pose.orientation.w = q_new[3] - return self.spawn_object( - pose=pose, object_name=object_name, item_stored=item_stored, frame=frame - ) + return self.make_spawn_entity_msg(pose, object_name, item_stored, frame) def clear_scene(self): wait_for_ros2_services(self.connector, ["/get_entities", "/delete_entity"]) From a4b6147d74679a71ae60cf8303833584a0f100f8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Wed, 8 Jul 2026 12:12:39 +0200 Subject: [PATCH 02/11] Add custom spawn service handler for SpawnEntities MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- pixi.toml | 1 + sim/Gem/CMakeLists.txt | 2 + sim/Gem/MobileManipulatorDemo_files.cmake | 2 + .../MobileManipulatorDemoSystemComponent.cpp | 2 + .../Source/SpawnEntitiesServiceHandler.cpp | 179 ++++++++++++++++++ sim/Gem/Source/SpawnEntitiesServiceHandler.h | 38 ++++ sim/Gem/Source/SpawnEntityServiceHandler.cpp | 58 +----- sim/Gem/Source/SpawnServiceUtils.cpp | 56 ++++++ sim/Gem/Source/SpawnServiceUtils.h | 6 + sim/Gem/mobilemanipulatordemo_files.cmake | 2 + sim/Registry/ros2.setreg | 3 +- 11 files changed, 291 insertions(+), 58 deletions(-) create mode 100644 sim/Gem/Source/SpawnEntitiesServiceHandler.cpp create mode 100644 sim/Gem/Source/SpawnEntitiesServiceHandler.h diff --git a/pixi.toml b/pixi.toml index 87f6f09..8ab7eb9 100644 --- a/pixi.toml +++ b/pixi.toml @@ -146,6 +146,7 @@ description = "Clone o3de-extras gems locally and validate all gem paths from pr # No cwd override: the script path is project-root-relative and the script cd's # into $DEMO_ROOT/sim itself. cmd = "bash scripts/build_sim.sh" +inputs = ["sim/Gem/Source/*", "sim/Gem/Include/*", "sim/Gem/CMakeLists.txt", "sim/Gem/gem.json", "sim/Gem/*.cmake"] outputs = ["sim/build/linux/bin/profile/MobileManipulatorDemo.GameLauncher"] # All gems are registered project-locally via external_subdirectories in # sim/project.json, so the gem source trees must exist before cmake configure. diff --git a/sim/Gem/CMakeLists.txt b/sim/Gem/CMakeLists.txt index fd8d973..6526dd0 100644 --- a/sim/Gem/CMakeLists.txt +++ b/sim/Gem/CMakeLists.txt @@ -33,6 +33,7 @@ ly_add_target( BUILD_DEPENDENCIES PRIVATE AZ::AzGameFramework + AZ::AzFramework Gem::Atom_AtomBridge.Static Gem::SimulationInterfaces.API Gem::ROS2.API @@ -51,6 +52,7 @@ ly_add_target( PRIVATE Gem::${gem_name}.Private.Object AZ::AzCore + AZ::AzFramework Gem::SimulationInterfaces.API Gem::ROS2.API ) diff --git a/sim/Gem/MobileManipulatorDemo_files.cmake b/sim/Gem/MobileManipulatorDemo_files.cmake index 87783bd..63c03e7 100644 --- a/sim/Gem/MobileManipulatorDemo_files.cmake +++ b/sim/Gem/MobileManipulatorDemo_files.cmake @@ -8,6 +8,8 @@ set(FILES Source/SpawnEntityServiceHandler.cpp Source/SpawnEntityServiceHandler.h + Source/SpawnEntitiesServiceHandler.cpp + Source/SpawnEntitiesServiceHandler.h Source/SpawnServiceUtils.cpp Source/SpawnServiceUtils.h ) diff --git a/sim/Gem/Source/MobileManipulatorDemoSystemComponent.cpp b/sim/Gem/Source/MobileManipulatorDemoSystemComponent.cpp index 0fd82a5..15e991b 100644 --- a/sim/Gem/Source/MobileManipulatorDemoSystemComponent.cpp +++ b/sim/Gem/Source/MobileManipulatorDemoSystemComponent.cpp @@ -7,6 +7,7 @@ #include #include "SpawnEntityServiceHandler.h" +#include "SpawnEntitiesServiceHandler.h" namespace MobileManipulatorDemo { @@ -82,6 +83,7 @@ namespace MobileManipulatorDemo } RegisterInterface(ros2Node); + RegisterInterface(ros2Node); } void MobileManipulatorDemoSystemComponent::DestroyHandlers() diff --git a/sim/Gem/Source/SpawnEntitiesServiceHandler.cpp b/sim/Gem/Source/SpawnEntitiesServiceHandler.cpp new file mode 100644 index 0000000..d05f153 --- /dev/null +++ b/sim/Gem/Source/SpawnEntitiesServiceHandler.cpp @@ -0,0 +1,179 @@ +// NOTE: this file is a slightly modified copy of SimulationInterfaces/Code/Source/Services/SpawnEntitiesServiceHandler.cpp + +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#include "SpawnEntitiesServiceHandler.h" +#include "SpawnServiceUtils.h" +#include +#include +#include +#include +#include +#include + +namespace MobileManipulatorDemo +{ + SpawnEntitiesServiceHandler::SpawnEntitiesServiceHandler() + { + ROS2SimulationInterfaces::ROS2SimulationInterfacesRequestBus::Broadcast( + &ROS2SimulationInterfaces::ROS2SimulationInterfacesRequests::AddSimulationFeatures, + AZStd::unordered_set{ + simulation_interfaces::msg::SimulatorFeatures::SPAWNING_ENTITIES }); + } + + AZStd::optional SpawnEntitiesServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) + { + const builtin_interfaces::msg::Time zeroTime = builtin_interfaces::msg::Time(); + const auto simulatorFrameId = ROS2SimulationInterfaces::RegistryUtilities::GetSimulatorROS2Frame(); + + Response response; + response.results.resize(request.spawn_requests.size()); + + AZStd::vector spawningEntities; + spawningEntities.reserve(request.spawn_requests.size()); + AZStd::vector spawnedRequestsIndices; + spawnedRequestsIndices.reserve(request.spawn_requests.size()); + + bool hasFailures = false; + for (size_t requestIdx = 0; requestIdx < request.spawn_requests.size(); ++requestIdx) + { + const auto& spawnRequest = request.spawn_requests[requestIdx]; + auto& spawnResult = response.results[requestIdx]; + + const AZStd::string_view name{ spawnRequest.name.c_str(), spawnRequest.name.size() }; + const AZStd::string_view uri{ spawnRequest.entity_resource.uri.c_str(), spawnRequest.entity_resource.uri.size() }; + const AZStd::string_view entityNamespace{ spawnRequest.entity_namespace.c_str(), spawnRequest.entity_namespace.size() }; + const AZStd::string_view messageFrameId{ spawnRequest.initial_pose.header.frame_id.c_str(), + spawnRequest.initial_pose.header.frame_id.size() }; + + if (!name.empty() && !SpawnServiceUtils::ValidateEntityName(name)) + { + spawnResult.result.result = simulation_interfaces::msg::SpawnResult::NAME_INVALID; + spawnResult.result.error_message = + "Invalid entity name. Entity names can only contain alphanumeric characters and underscores."; + hasFailures = true; + continue; + } + + if (!entityNamespace.empty() && !SpawnServiceUtils::ValidateNamespaceName(entityNamespace)) + { + spawnResult.result.result = simulation_interfaces::msg::SpawnResult::NAMESPACE_INVALID; + spawnResult.result.error_message = + "Invalid entity namespace. Entity namespaces can only contain alphanumeric characters and forward slashes."; + hasFailures = true; + continue; + } + + AZ::Transform transformOffset = AZ::Transform::CreateIdentity(); + if (!messageFrameId.empty() && simulatorFrameId != messageFrameId) + { + auto transformInterface = ROS2::TFInterface::Get(); + AZ_Assert(transformInterface, "TFInterface is not available, cannot set entity state without transform offset."); + const auto transformOutcome = transformInterface->GetTransform(simulatorFrameId, messageFrameId, zeroTime); + + if (transformOutcome.IsSuccess()) + { + transformOffset = transformOutcome.GetValue(); + } + else + { + spawnResult.result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED; + spawnResult.result.error_message = transformOutcome.GetError().c_str(); + hasFailures = true; + continue; + } + } + + const AZ::Transform requestedPose = ROS2::ROS2Conversions::FromROS2Pose(spawnRequest.initial_pose.pose); + if (const auto poseValidation = SpawnServiceUtils::ValidateTransformNormalized(requestedPose); !poseValidation.IsSuccess()) + { + spawnResult.result.result = simulation_interfaces::msg::SpawnResult::INVALID_POSE; + spawnResult.result.error_message = poseValidation.GetError().c_str(); + hasFailures = true; + continue; + } + + const AZ::Transform initialPose = transformOffset * + AZ::Transform::CreateFromQuaternionAndTranslation( + requestedPose.GetRotation().GetNormalized(), requestedPose.GetTranslation()); + + SimulationInterfaces::SpawningEntity spawningEntity; + spawningEntity.name = AZStd::string(name); + spawningEntity.uri = AZStd::string(uri); + spawningEntity.entityNamespace = AZStd::string(entityNamespace); + spawningEntity.initialPose = initialPose; + spawningEntity.allowRename = spawnRequest.allow_renaming; + spawningEntity.preinsertionCb = + [](const AZ::Outcome&) + { + }; + spawningEntity.completedCb = [](const AZ::Outcome&) + { + }; + + spawningEntities.push_back(AZStd::move(spawningEntity)); + spawnedRequestsIndices.push_back(requestIdx); + } + + if (spawningEntities.empty()) + { + response.result.result = hasFailures ? simulation_interfaces::srv::SpawnEntities::Response::ENTITIES_SPAWN_FAILED + : simulation_interfaces::msg::Result::RESULT_OK; + if (hasFailures) + { + response.result.error_message = "One or more entity spawn requests failed."; + } + SendResponse(response); + return AZStd::nullopt; + } + + SimulationInterfaces::SimulationEntityManagerRequestBus::Broadcast( + &SimulationInterfaces::SimulationEntityManagerRequests::SpawnEntities, + spawningEntities, + [this, response, spawnedRequestsIndices, hasFailures](const SimulationInterfaces::BatchSpawnResult& batchResult) mutable + { + bool hasBatchFailures = hasFailures; + + for (size_t spawnedIdx = 0; spawnedIdx < batchResult.m_spawnResults.size() && spawnedIdx < spawnedRequestsIndices.size(); + ++spawnedIdx) + { + const size_t requestIdx = spawnedRequestsIndices[spawnedIdx]; + auto& spawnResult = response.results[requestIdx]; + const auto& outcome = batchResult.m_spawnResults[spawnedIdx]; + + if (outcome.IsSuccess()) + { + spawnResult.result.result = simulation_interfaces::msg::Result::RESULT_OK; + spawnResult.entity_name = outcome.GetValue().c_str(); + SpawnServiceUtils::RegisterChildGrippingPoints(outcome.GetValue()); + } + else + { + const auto& failedResult = outcome.GetError(); + spawnResult.result.result = failedResult.m_errorCode; + spawnResult.result.error_message = failedResult.m_errorString.c_str(); + hasBatchFailures = true; + } + } + + response.result.result = hasBatchFailures ? simulation_interfaces::srv::SpawnEntities::Response::ENTITIES_SPAWN_FAILED + : simulation_interfaces::msg::Result::RESULT_OK; + if (hasBatchFailures) + { + response.result.error_message = "One or more entity spawn requests failed."; + } + + SendResponse(response); + }); + + return AZStd::nullopt; + } + +} // namespace MobileManipulatorDemo diff --git a/sim/Gem/Source/SpawnEntitiesServiceHandler.h b/sim/Gem/Source/SpawnEntitiesServiceHandler.h new file mode 100644 index 0000000..6d9a902 --- /dev/null +++ b/sim/Gem/Source/SpawnEntitiesServiceHandler.h @@ -0,0 +1,38 @@ +// NOTE: this file is a slightly modified copy of SimulationInterfaces/Code/Source/Services/SpawnEntitiesServiceHandler.h + +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#pragma once + +#include "ROS2Service.h" +#include +#include + +namespace MobileManipulatorDemo +{ + class SpawnEntitiesServiceHandler + : public ROS2Service + { + public: + SpawnEntitiesServiceHandler(); + + AZStd::string_view GetTypeName() const override + { + return "SpawnEntities"; + } + + AZStd::string_view GetDefaultName() const override + { + return "spawn_entities"; + } + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; + }; + +} // namespace MobileManipulatorDemo diff --git a/sim/Gem/Source/SpawnEntityServiceHandler.cpp b/sim/Gem/Source/SpawnEntityServiceHandler.cpp index 9ef8ce4..daef8f1 100644 --- a/sim/Gem/Source/SpawnEntityServiceHandler.cpp +++ b/sim/Gem/Source/SpawnEntityServiceHandler.cpp @@ -9,12 +9,8 @@ */ #include "SpawnEntityServiceHandler.h" -#include -#include -#include #include #include "SpawnServiceUtils.h" -#include #include #include #include @@ -25,58 +21,6 @@ namespace MobileManipulatorDemo { - AZStd::vector GetAllDescendants(AZ::EntityId parent) - { - AZStd::vector descendants; - AZ::TransformBus::EventResult( - descendants, - parent, - &AZ::TransformInterface::GetAllDescendants); - - return descendants; - } - - AZStd::string GetEntityName(AZ::EntityId entityId) - { - AZStd::string name; - AZ::ComponentApplicationBus::BroadcastResult( - name, - &AZ::ComponentApplicationRequests::GetEntityName, - entityId - ); - - return name; - } - - void RegisterChildGrippingPoints(const AZStd::string& rootName) - { - AZ::Outcome rootId; - SimulationInterfaces::SimulationEntityManagerRequestBus::BroadcastResult( - rootId, - &SimulationInterfaces::SimulationEntityManagerRequests::GetEntityRoot, - rootName - ); - - if (rootId.IsSuccess()) - { - for (auto& descendantId : GetAllDescendants(rootId.GetValue())) - { - auto descendantName = GetEntityName(descendantId); - - if (descendantName.contains("GrippingPoint")) - { - auto proposedName = rootName + "_" + descendantName; - AZ::Outcome result; - SimulationInterfaces::SimulationEntityManagerRequestBus::BroadcastResult( - result, - &SimulationInterfaces::SimulationEntityManagerRequests::RegisterNewSimulatedBody, - proposedName, - descendantId - ); - } - } - } - } SpawnEntityServiceHandler::SpawnEntityServiceHandler() { @@ -189,7 +133,7 @@ namespace MobileManipulatorDemo Response response; if (outcome.IsSuccess()) { - RegisterChildGrippingPoints(outcome.GetValue()); + SpawnServiceUtils::RegisterChildGrippingPoints(outcome.GetValue()); response.result.result = simulation_interfaces::msg::Result::RESULT_OK; response.entity_name = outcome.GetValue().c_str(); } diff --git a/sim/Gem/Source/SpawnServiceUtils.cpp b/sim/Gem/Source/SpawnServiceUtils.cpp index a36843e..c6e2533 100644 --- a/sim/Gem/Source/SpawnServiceUtils.cpp +++ b/sim/Gem/Source/SpawnServiceUtils.cpp @@ -11,6 +11,9 @@ #include "SpawnServiceUtils.h" #include #include +#include +#include +#include namespace MobileManipulatorDemo::SpawnServiceUtils { @@ -54,4 +57,57 @@ namespace MobileManipulatorDemo::SpawnServiceUtils return AZ::Success(); } + + AZStd::vector GetAllDescendants(AZ::EntityId parent) + { + AZStd::vector descendants; + AZ::TransformBus::EventResult( + descendants, + parent, + &AZ::TransformInterface::GetAllDescendants); + + return descendants; + } + + AZStd::string GetEntityName(AZ::EntityId entityId) + { + AZStd::string name; + AZ::ComponentApplicationBus::BroadcastResult( + name, + &AZ::ComponentApplicationRequests::GetEntityName, + entityId + ); + + return name; + } + + void RegisterChildGrippingPoints(const AZStd::string& rootName) + { + AZ::Outcome rootId; + SimulationInterfaces::SimulationEntityManagerRequestBus::BroadcastResult( + rootId, + &SimulationInterfaces::SimulationEntityManagerRequests::GetEntityRoot, + rootName + ); + + if (rootId.IsSuccess()) + { + for (auto& descendantId : GetAllDescendants(rootId.GetValue())) + { + auto descendantName = GetEntityName(descendantId); + + if (descendantName.contains("GrippingPoint")) + { + auto proposedName = rootName + "_" + descendantName; + AZ::Outcome result; + SimulationInterfaces::SimulationEntityManagerRequestBus::BroadcastResult( + result, + &SimulationInterfaces::SimulationEntityManagerRequests::RegisterNewSimulatedBody, + proposedName, + descendantId + ); + } + } + } + } } // namespace MobileManipulatorDemo::SpawnServiceUtils diff --git a/sim/Gem/Source/SpawnServiceUtils.h b/sim/Gem/Source/SpawnServiceUtils.h index 4045df2..834ffba 100644 --- a/sim/Gem/Source/SpawnServiceUtils.h +++ b/sim/Gem/Source/SpawnServiceUtils.h @@ -14,6 +14,8 @@ #include #include #include +#include +#include namespace MobileManipulatorDemo::SpawnServiceUtils { @@ -27,4 +29,8 @@ namespace MobileManipulatorDemo::SpawnServiceUtils //! @return Success when valid, Failure with a descriptive error message when not. AZ::Outcome ValidateTransformNormalized( const AZ::Transform& transform, float quaternionTolerance = 1e-3f, float maxTranslation = 1e7f); + + AZStd::vector GetAllDescendants(AZ::EntityId parent); + AZStd::string GetEntityName(AZ::EntityId entityId); + void RegisterChildGrippingPoints(const AZStd::string& rootName); } // namespace MobileManipulatorDemo::SpawnServiceUtils diff --git a/sim/Gem/mobilemanipulatordemo_files.cmake b/sim/Gem/mobilemanipulatordemo_files.cmake index 63d86a3..921a89d 100644 --- a/sim/Gem/mobilemanipulatordemo_files.cmake +++ b/sim/Gem/mobilemanipulatordemo_files.cmake @@ -7,6 +7,8 @@ set(FILES Source/SpawnEntityServiceHandler.cpp Source/SpawnEntityServiceHandler.h + Source/SpawnEntitiesServiceHandler.cpp + Source/SpawnEntitiesServiceHandler.h Source/SpawnServiceUtils.cpp Source/SpawnServiceUtils.h ) diff --git a/sim/Registry/ros2.setreg b/sim/Registry/ros2.setreg index 7670f4b..f1666e1 100644 --- a/sim/Registry/ros2.setreg +++ b/sim/Registry/ros2.setreg @@ -24,7 +24,8 @@ }, "HandlersNames": { - "SpawnEntity": "" + "SpawnEntity": "", + "SpawnEntities": "" } } } From 63093a094bdcf0eb1e3c2a963e8e4c6d24053172 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Wed, 8 Jul 2026 12:32:19 +0200 Subject: [PATCH 03/11] pre-commit MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- rai_app/environment/scene_manager.py | 18 ++++-------------- 1 file changed, 4 insertions(+), 14 deletions(-) diff --git a/rai_app/environment/scene_manager.py b/rai_app/environment/scene_manager.py index 051f02c..0cb1f1a 100644 --- a/rai_app/environment/scene_manager.py +++ b/rai_app/environment/scene_manager.py @@ -224,7 +224,7 @@ def init_spawn_entity_name_and_pose( pose: Pose, object_name: str, item_stored: Optional[str] = None, - frame: str = "odom" + frame: str = "odom", ): # NOTE (jmatejcz) item stored will be added to name of object # and that's how it will be distinguished @@ -245,22 +245,15 @@ def init_spawn_entity_name_and_pose( return req - def make_spawn_entity_msg( self, pose: Pose, object_name: str, item_stored: Optional[str] = None, - frame: str = "odom" + frame: str = "odom", ) -> SpawnEntityMsg: req = SpawnEntityMsg() - self.init_spawn_entity_name_and_pose( - req, - pose, - object_name, - item_stored, - frame - ) + self.init_spawn_entity_name_and_pose(req, pose, object_name, item_stored, frame) req.entity_resource.uri = self.spawnable_to_uri[object_name] return req @@ -288,10 +281,7 @@ def spawn_object( result = cast(SpawnEntity.Response, result) return req.name - def spawn_objects( - self, - requests: list[SpawnEntityMsg] - ): + def spawn_objects(self, requests: list[SpawnEntityMsg]): wait_for_ros2_services(self.connector, ["/spawn_entities"]) req = SpawnEntities.Request() From 743de23d69c560fd672bc11bf5bf859ae7ce01c3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Wed, 8 Jul 2026 14:11:17 +0200 Subject: [PATCH 04/11] Remove unused dependency MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- pixi.toml | 2 +- sim/Gem/CMakeLists.txt | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/pixi.toml b/pixi.toml index 8ab7eb9..c8030e5 100644 --- a/pixi.toml +++ b/pixi.toml @@ -146,7 +146,7 @@ description = "Clone o3de-extras gems locally and validate all gem paths from pr # No cwd override: the script path is project-root-relative and the script cd's # into $DEMO_ROOT/sim itself. cmd = "bash scripts/build_sim.sh" -inputs = ["sim/Gem/Source/*", "sim/Gem/Include/*", "sim/Gem/CMakeLists.txt", "sim/Gem/gem.json", "sim/Gem/*.cmake"] +inputs = ["scripts/build_sim.sh", "sim/Gem/Source/*", "sim/Gem/Include/*", "sim/Gem/CMakeLists.txt", "sim/Gem/gem.json", "sim/Gem/*.cmake"] outputs = ["sim/build/linux/bin/profile/MobileManipulatorDemo.GameLauncher"] # All gems are registered project-locally via external_subdirectories in # sim/project.json, so the gem source trees must exist before cmake configure. diff --git a/sim/Gem/CMakeLists.txt b/sim/Gem/CMakeLists.txt index 6526dd0..fd8d973 100644 --- a/sim/Gem/CMakeLists.txt +++ b/sim/Gem/CMakeLists.txt @@ -33,7 +33,6 @@ ly_add_target( BUILD_DEPENDENCIES PRIVATE AZ::AzGameFramework - AZ::AzFramework Gem::Atom_AtomBridge.Static Gem::SimulationInterfaces.API Gem::ROS2.API @@ -52,7 +51,6 @@ ly_add_target( PRIVATE Gem::${gem_name}.Private.Object AZ::AzCore - AZ::AzFramework Gem::SimulationInterfaces.API Gem::ROS2.API ) From 921216ffdbfece9dba4e0031dd05a92ccfbaef5f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Fri, 10 Jul 2026 15:15:09 +0200 Subject: [PATCH 05/11] Use batch spawning in housekeep scenario MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- rai_app/environment/scene_agent.py | 23 +++++++++-------------- 1 file changed, 9 insertions(+), 14 deletions(-) diff --git a/rai_app/environment/scene_agent.py b/rai_app/environment/scene_agent.py index d9906a3..179c272 100644 --- a/rai_app/environment/scene_agent.py +++ b/rai_app/environment/scene_agent.py @@ -330,20 +330,15 @@ def housekeep_scenario(self, request: Trigger.Request, response: Trigger.Respons self.get_standard_objects_to_spawn(rack_fill=self.rack_fill) ) - for slot, entity_type, item in tqdm( - zip(spawn_slot_names, spawn_entity_types, items_stored), - desc="Spawning entities", - total=len(spawn_slot_names), - ): - self.spawn_on_spot( - slot_name=slot, - object_name=entity_type, - item_stored=item, - std_xy=0.01, - std_yaw=0.05, - rotate_90_degrees=True, - rotate_90_degrees_percentage=0.15, - ) + self.populate_scene( + spawn_slot_names, + spawn_entity_types, + items_stored, + std_xy=0.01, + std_yaw=0.05, + percent_of_rotated_objects=0.15, + ) + spawn_slot_names, spawn_entity_types, items_stored = ( self.get_returns_table_objects_to_spawn() ) From e699eeb5357bbd9552bd14d044333d33bc2a5dd1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Fri, 10 Jul 2026 15:37:37 +0200 Subject: [PATCH 06/11] Move random rotation to spawn function MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- rai_app/environment/scene_agent.py | 23 +++++++++-------------- rai_app/environment/scene_manager.py | 15 +++++++-------- 2 files changed, 16 insertions(+), 22 deletions(-) diff --git a/rai_app/environment/scene_agent.py b/rai_app/environment/scene_agent.py index 179c272..86bd4c0 100644 --- a/rai_app/environment/scene_agent.py +++ b/rai_app/environment/scene_agent.py @@ -357,20 +357,15 @@ def standard_scenario(self, request: Trigger.Request, response: Trigger.Response self.get_standard_objects_to_spawn(rack_fill=self.rack_fill) ) - for slot, entity_type, item in tqdm( - zip(spawn_slot_names, spawn_entity_types, items_stored), - desc="Spawning entities", - total=len(spawn_slot_names), - ): - self.spawn_on_spot( - slot_name=slot, - object_name=entity_type, - item_stored=item, - std_xy=0.01, - std_yaw=0.05, - rotate_90_degrees=True, - rotate_90_degrees_percentage=0.03, - ) + self.populate_scene( + spawn_slot_names, + spawn_entity_types, + items_stored, + std_xy=0.01, + std_yaw=0.05, + percent_of_rotated_objects=0.03, + ) + spawn_slot_names, spawn_entity_types, items_stored = ( self.get_returns_table_objects_to_spawn() ) diff --git a/rai_app/environment/scene_manager.py b/rai_app/environment/scene_manager.py index 0cb1f1a..c980fa3 100644 --- a/rai_app/environment/scene_manager.py +++ b/rai_app/environment/scene_manager.py @@ -197,14 +197,6 @@ def populate_scene( reqs: list[SpawnEntityMsg] = [] for slot, object_name, item in zip(slots, object_names, items_stored): - if np.isclose(offset_yaw, 0.0): - should_rotate = random.random() < percent_of_rotated_objects - if should_rotate: - # rotate additional 90 degrees - offset_yaw = random.choice([1.57, -1.57, 3.14]) - else: - offset_yaw = 0.0 - req = self.make_spawn_entity_msg_for_spot( slot_name=slot, object_name=object_name, @@ -212,6 +204,8 @@ def populate_scene( std_xy=std_xy, std_yaw=std_yaw, offset_yaw=offset_yaw, + rotate_90_degrees=not bool(np.isclose(0.0, percent_of_rotated_objects)), + rotate_90_degrees_percentage=percent_of_rotated_objects, ) reqs.append(req) @@ -308,6 +302,8 @@ def make_spawn_entity_msg_for_spot( item_stored: Optional[str] = None, std_xy: float = 0.0, std_yaw: float = 0.0, + rotate_90_degrees: bool = False, + rotate_90_degrees_percentage: float = 0.1, offset_yaw: float = 0.0, frame: str = "odom", ) -> SpawnEntityMsg: @@ -322,6 +318,9 @@ def make_spawn_entity_msg_for_spot( # Add Gaussian noise to yaw yaw += random.normalvariate(0, std_yaw) + if rotate_90_degrees: + if random.random() < rotate_90_degrees_percentage: + yaw += random.choice([-np.pi / 2, np.pi / 2]) yaw += offset_yaw # Convert back to quaternion From 6f0b073de4c495ef3d82aab994a7f21c90878989 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Fri, 10 Jul 2026 17:22:32 +0200 Subject: [PATCH 07/11] Remove magic number and use self.logger MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- rai_app/environment/scene_manager.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rai_app/environment/scene_manager.py b/rai_app/environment/scene_manager.py index c980fa3..a35e4ae 100644 --- a/rai_app/environment/scene_manager.py +++ b/rai_app/environment/scene_manager.py @@ -32,7 +32,7 @@ wait_for_ros2_services, ) from rosidl_runtime_py.convert import message_to_ordereddict -from simulation_interfaces.msg import EntityState +from simulation_interfaces.msg import EntityState, Result from simulation_interfaces.msg import SpawnEntity as SpawnEntityMsg from simulation_interfaces.srv import ( GetEntities, @@ -291,8 +291,8 @@ def spawn_objects(self, requests: list[SpawnEntityMsg]): ).payload result = cast(SpawnEntities.Response, result) for res in result.results: - if res.result.result != 1: - print(f"ERROR: {res.result.error_message}") + if res.result.result != Result.RESULT_OK: + self.logger.debug(f"ERROR: {res.result.error_message}") return [res.entity_name for res in result.results] def make_spawn_entity_msg_for_spot( From 377d7a79ec3dbb91fecd2c2901e48598c446b85f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Fri, 10 Jul 2026 17:37:43 +0200 Subject: [PATCH 08/11] Use constant for spawn request timeout MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- rai_app/environment/scene_manager.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/rai_app/environment/scene_manager.py b/rai_app/environment/scene_manager.py index a35e4ae..298def2 100644 --- a/rai_app/environment/scene_manager.py +++ b/rai_app/environment/scene_manager.py @@ -62,6 +62,9 @@ class Collection(Enum): FREE = "t3" +SPAWN_ENTITY_REQUEST_TIMEOUT = 3.0 + + class SceneManager: def __init__( self, @@ -269,7 +272,7 @@ def spawn_object( ROS2Message(payload=message_to_ordereddict(req)), target="/spawn_entity", msg_type="simulation_interfaces/srv/SpawnEntity", - timeout_sec=3.0, + timeout_sec=SPAWN_ENTITY_REQUEST_TIMEOUT, reuse_client=True, ).payload result = cast(SpawnEntity.Response, result) @@ -286,7 +289,7 @@ def spawn_objects(self, requests: list[SpawnEntityMsg]): ROS2Message(payload=message_to_ordereddict(req)), target="/spawn_entities", msg_type="simulation_interfaces/srv/SpawnEntities", - timeout_sec=3.0, + timeout_sec=SPAWN_ENTITY_REQUEST_TIMEOUT, reuse_client=True, ).payload result = cast(SpawnEntities.Response, result) From 80e5bcce5591c898b41eb58e15945879999c9683 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Mon, 13 Jul 2026 12:40:36 +0200 Subject: [PATCH 09/11] Recursive path for build-sim input files MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- pixi.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pixi.toml b/pixi.toml index 07430b3..a7918cf 100644 --- a/pixi.toml +++ b/pixi.toml @@ -146,7 +146,7 @@ description = "Clone o3de-extras gems locally and validate all gem paths from pr # No cwd override: the script path is project-root-relative and the script cd's # into $DEMO_ROOT/sim itself. cmd = "bash scripts/build_sim.sh" -inputs = ["scripts/build_sim.sh", "sim/Gem/Source/*", "sim/Gem/Include/*", "sim/Gem/CMakeLists.txt", "sim/Gem/gem.json", "sim/Gem/*.cmake"] +inputs = ["scripts/build_sim.sh", "sim/Gem/Source/**", "sim/Gem/Include/**", "sim/Gem/CMakeLists.txt", "sim/Gem/gem.json", "sim/Gem/*.cmake"] outputs = ["sim/build/linux/bin/profile/MobileManipulatorDemo.GameLauncher"] # All gems are registered project-locally via external_subdirectories in # sim/project.json, so the gem source trees must exist before cmake configure. From a890359a488aa7ef4937b8b035a44a3354a327b9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Mon, 13 Jul 2026 15:22:49 +0200 Subject: [PATCH 10/11] Add spawn_on_spot method back MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- rai_app/environment/scene_manager.py | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/rai_app/environment/scene_manager.py b/rai_app/environment/scene_manager.py index 78be849..b5fbad6 100644 --- a/rai_app/environment/scene_manager.py +++ b/rai_app/environment/scene_manager.py @@ -298,6 +298,27 @@ def spawn_objects(self, requests: list[SpawnEntityMsg]): self.logger.debug(f"ERROR: {res.result.error_message}") return [res.entity_name for res in result.results] + def spawn_on_spot( + self, + slot_name: str, + object_name: str, + item_stored: Optional[str] = None, + std_xy: float = 0.0, + std_yaw: float = 0.0, + offset_yaw: float = 0.0, + frame: str = "odom", + ): + msg = self.make_spawn_entity_msg_for_spot( + slot_name, + object_name, + item_stored, + std_xy, + std_yaw, + offset_yaw=offset_yaw, + frame=frame, + ) + return self.spawn_objects([msg])[0] + def make_spawn_entity_msg_for_spot( self, slot_name: str, From 92ec02290e80f58c9b29971815f24a2a2d28122e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Mon, 13 Jul 2026 17:45:21 +0200 Subject: [PATCH 11/11] Bump llama.cpp submodule MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- inference/llama.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/inference/llama.cpp b/inference/llama.cpp index 6f4f53f..e920c52 160000 --- a/inference/llama.cpp +++ b/inference/llama.cpp @@ -1 +1 @@ -Subproject commit 6f4f53f2b7da54fcdbbecaaa734337c337ad6176 +Subproject commit e920c523e3b8a0163fe498af5bf90df35ff51d25