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 diff --git a/pixi.toml b/pixi.toml index 225a7b0..a7918cf 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 = ["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/rai_app/environment/scene_agent.py b/rai_app/environment/scene_agent.py index d9906a3..86bd4c0 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() ) @@ -362,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 f6771cc..b5fbad6 100644 --- a/rai_app/environment/scene_manager.py +++ b/rai_app/environment/scene_manager.py @@ -32,12 +32,15 @@ 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, GetEntityState, SetEntityState, + SpawnEntities, SpawnEntity, + SpawnEntity_Request, ) from tf2_geometry_msgs import do_transform_pose from tf_transformations import euler_from_quaternion, quaternion_from_euler @@ -59,6 +62,9 @@ class Collection(Enum): FREE = "t3" +SPAWN_ENTITY_REQUEST_TIMEOUT = 3.0 + + class SceneManager: def __init__( self, @@ -192,40 +198,31 @@ 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), - ): - 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 - - simulation_name = self.spawn_on_spot( + reqs: list[SpawnEntityMsg] = [] + for slot, object_name, item in zip(slots, object_names, items_stored): + req = self.make_spawn_entity_msg_for_spot( slot_name=slot, object_name=object_name, item_stored=item, 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, ) - 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", ): - 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 +230,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,16 +240,63 @@ 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", 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) - return name + return req.name + + def spawn_objects(self, requests: list[SpawnEntityMsg]): + wait_for_ros2_services(self.connector, ["/spawn_entities"]) + + 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=SPAWN_ENTITY_REQUEST_TIMEOUT, + reuse_client=True, + ).payload + result = cast(SpawnEntities.Response, result) + for res in result.results: + 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 spawn_on_spot( self, @@ -266,7 +308,29 @@ def spawn_on_spot( offset_yaw: float = 0.0, frame: str = "odom", ): - wait_for_ros2_services(self.connector, ["/spawn_entity"]) + 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, + object_name: str, + 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: pose: Pose = copy.deepcopy(self.slots[slot_name].origin_pose) # Add Gaussian noise to x, y pose.position.x += random.normalvariate(0, std_xy) @@ -278,6 +342,9 @@ def spawn_on_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 @@ -287,9 +354,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"]) 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": "" } } }