diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..f9f6b0d --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 3.16) +project(simulation_interfaces) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +set(msg_files + "msg/Bounds.msg" + "msg/EntityCategory.msg" + "msg/EntityFilters.msg" + "msg/EntityInfo.msg" + "msg/EntityState.msg" + "msg/NamedPose.msg" + "msg/Result.msg" + "msg/SimulationState.msg" + "msg/SimulatorFeatures.msg" + "msg/Spawnable.msg" + "msg/TagsFilter.msg" +) + +set(srv_files + "srv/DeleteEntity.srv" + "srv/GetEntities.srv" + "srv/GetEntitiesStates.srv" + "srv/GetEntityBounds.srv" + "srv/GetEntityInfo.srv" + "srv/GetEntityState.srv" + "srv/GetNamedPoseBounds.srv" + "srv/GetNamedPoses.srv" + "srv/GetSimulationState.srv" + "srv/GetSimulatorFeatures.srv" + "srv/GetSpawnables.srv" + "srv/ResetSimulation.srv" + "srv/SetEntityInfo.srv" + "srv/SetEntityState.srv" + "srv/SetSimulationState.srv" + "srv/SpawnEntity.srv" + "srv/StepSimulation.srv" +) + +set(action_files + "action/SimulateSteps.action" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + ${action_files} + DEPENDENCIES builtin_interfaces std_msgs geometry_msgs + ADD_LINTER_TESTS +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/README.md b/README.md index 1746f20..55c9481 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,25 @@ -# simulation_interfaces -Standard interfaces for interacting with simulators from ROS 2 +# Simulation Interfaces + +Standard ROS 2 interfaces for interacting with simulators. +Messages, services, and actions are documented in their respective files. + +## Result.msg + +The standard includes a generic message for handling service responses, [Result.msg](msg/Result.msg), +which will likely not be included in the final version of the standard. Since it is generic, it will either be promoted to a common message or included in the +service mechanism itself. + +## Suggested interface implementation priorities + +[GetSimulatorFeatures](msg/GetSimulationFeatures.msg) should be implemented first, as it provides users with information about +the state of support for all standard simulation interfaces. + +Following that, aim for maintaining consistency within the implemented feature, such as enabling both +_Get_ and _Set_ parts. + +Some interfaces represent optional utility and are considered lower priority: +- [GetEntityBounds](srv/GetEntityBounds.srv) +- [GetNamedPoseBounds](srv/GetNamedPoseBounds.srv) +- [GetNamedPoses](srv/GetNamedPoses.srv) +- [GetSpawnables](srv/GetSpawnables.srv) +- [SetEntityInfo](srv/SetEntityInfo.srv) \ No newline at end of file diff --git a/action/SimulateSteps.action b/action/SimulateSteps.action new file mode 100644 index 0000000..4ba9ab7 --- /dev/null +++ b/action/SimulateSteps.action @@ -0,0 +1,13 @@ +# Assuming the simulation is paused, simulate a finite number of steps and return to paused state. +# The action returns feedback after each complete step. +# For a service alternative, see StepSimulation, which often makes more sense when doing a single step (steps = 1). +# Support for this interface is indicated through the STEP_SIMULATION_ACTION value in GetSimulationFeatures. + +uint64 steps # Action goal: step through the simulation loop this many times. +--- + +Result result # Calling with simulation unpaused will return OPERATION_FAILED and error message. + +--- +uint64 completed_steps # number of completed steps in this action so far. +uint64 remaining_steps # number of steps remaining to be completed in this action. diff --git a/msg/Bounds.msg b/msg/Bounds.msg new file mode 100644 index 0000000..ed16b89 --- /dev/null +++ b/msg/Bounds.msg @@ -0,0 +1,25 @@ +# Bounds which are useful in several contexts, e.g. to avoid spawning with other object overlap, +# or parking in a spot that is too small. +# Certain goals or points might be valid for a small object, but not suitable for a large one, +# or a differently shaped one. +# Bounds can be also checked to ensure certain scenario conditions are met. +# For entities, these limits are relative to entity's canonical link transform, following ROS rep-103 convention. + +# As bounds are optional in most interfaces, TYPE_EMPTY signifies empty bounds, to be understood as "unbounded". +# Otherwise, the fields are expected to define a valid volume. +# For spawning with a named pose, you should check whether the entity simulation model fits within the bounds +# before calling SpawnEntity, to avoid overlaps and unstable behavior. + +# bounds type +uint8 TYPE_EMPTY = 0 # No bounds. The points vector will be empty. +uint8 TYPE_BOX = 1 # Axis-aligned bounding box, points field should have two values, + # which are upper right and lower left corners of the box. +uint8 TYPE_CONVEX_HULL = 2 # Points define a convex hull (at least 3 non-collinear points). +uint8 TYPE_SPHERE = 3 # A sphere with center and radius. First element of points vector is the center. + # The x field of the second point of the vector is the radius (y and z are ignored). + + +uint8 type +geometry_msgs/Vector3[] points # Points defining the bounded area. Check type field to determine semantics. + # Valid sizes are zero (no bounds), 2 (sphere or box, depending on type field), + # and 3 or more (convex hull). diff --git a/msg/EntityCategory.msg b/msg/EntityCategory.msg new file mode 100644 index 0000000..63d3c0f --- /dev/null +++ b/msg/EntityCategory.msg @@ -0,0 +1,16 @@ +# Entity major category, which often warrants a specific way to handle such entity, e.g. when handling humans +# or mapping persistence for dynamic vs static objects. + +uint8 CATEGORY_OBJECT = 0 # Generic or unspecified type. +uint8 CATEGORY_ROBOT = 1 # A broad category for mobile robots, arms, drones etc., + # usually with ROS 2 interfaces. +uint8 CATEGORY_HUMAN = 2 # Simulated humans, e.g. pedestrians, warehouse workers. + # Compared to CATEGORY_DYNAMIC_OBJECT, humans are often expected to be treated + # exceptionally in regards to safety constraints. +uint8 CATEGORY_DYNAMIC_OBJECT = 4 # Vehicles, animals, mobile obstacles, typically to present a detection and + # tracking challenge, such as when simulation is used to test robot perception + # or navigation stack. +uint8 CATEGORY_STATIC_OBJECT = 5 # Any object which is static, e.g. not supposed to change its pose + # unless by means of SetEntityState. + +uint8 category \ No newline at end of file diff --git a/msg/EntityFilters.msg b/msg/EntityFilters.msg new file mode 100644 index 0000000..967ffac --- /dev/null +++ b/msg/EntityFilters.msg @@ -0,0 +1,25 @@ +# A set of filters to apply to entity queries. See GetEntities, GetEntitiesStates. + +string filter # Optional, defaults to empty. Return entities with matching names. + # Entity names are matched with the filter regular expression. + # An empty filter will result in all entities being returned. + # The regular expression syntax is POSIX Extended, + # see https://pubs.opengroup.org/onlinepubs/9799919799/ definitions. + # Applies together with other filters (categories, tags). +EntityCategory[] categories # Optional, defaults to empty, which means no category filter. + # Entities matching any of the categories will be returned. + # To get entity category, use GetEntityInfo. + # Applies together with other filters (filter, tags). + # Check ENTITY_CATEGORIES in GetSimulatorFeatures to determine if + # your simulator supports entity categories. +TagsFilter tags # Tags filter to apply. To get entity tags, use GetEntityInfo. + # Applies together with other filters (filter, categories). + # Check support for this feature (ENTITY_TAGS) in GetSimulationFeatures. +Bounds bounds # if bounds are not empty, the overlap filter is applied + # and entities overlapping with bounds will be returned. + # Note that not all bound types might be supported by the simulator, + # though at least TYPE_SPHERE needs to be supported. + # Check ENTITY_BOUNDS_BOX and ENTITY_BOUNDS_CONVEX in GetSimulationFeatures + # to determine whether your simulator supports other bound types. + # If service is called with filter bounds set to an unsupported type, + # a FEATURE_UNSUPPORTED result will be returned. diff --git a/msg/EntityInfo.msg b/msg/EntityInfo.msg new file mode 100644 index 0000000..eeb7fdc --- /dev/null +++ b/msg/EntityInfo.msg @@ -0,0 +1,5 @@ +# Entity type and additional information + +EntityCategory category # Major category for the entity. Extra entity type distinction can be made through tags. +string description # optional: verbose, human-readable description of the entity. +string[] tags # optional: tags which are useful for filtering and categorizing entities further. diff --git a/msg/EntityState.msg b/msg/EntityState.msg new file mode 100644 index 0000000..c1a128d --- /dev/null +++ b/msg/EntityState.msg @@ -0,0 +1,8 @@ +# Entity current pose, twist and acceleration + +std_msgs/Header header # Reference frame and timestamp for pose and twist. Empty frame defaults to world. +geometry_msgs/Pose pose # Pose in reference frame, ground truth. +geometry_msgs/Twist twist # Ground truth linear and angular velocities + # observed in the frame specified by header.frame_id + # See https://github.com/ros2/common_interfaces/pull/240 for conventions. +geometry_msgs/Accel acceleration # Linear and angular acceleration ground truth, following the same convention. diff --git a/msg/NamedPose.msg b/msg/NamedPose.msg new file mode 100644 index 0000000..54dec31 --- /dev/null +++ b/msg/NamedPose.msg @@ -0,0 +1,8 @@ +# A named pose defined in the simulation for certain purposes such as spawning. + +string name # Unique name. +string description # Description for the user, e.g. "near the charging station". +string[] tags # Optional tags which can be used to determine the named pose + # purpose, for example: "spawn", "parking", "navigation_goal", + # as well as fitting entity types e.g. "drone", "turtlebot3". +geometry_msgs/Pose pose # Pose relative to world, which can be used with SpawnEntity.srv. diff --git a/msg/Result.msg b/msg/Result.msg new file mode 100644 index 0000000..64f3879 --- /dev/null +++ b/msg/Result.msg @@ -0,0 +1,18 @@ +# Result code and message for service calls. +# Note that additional results for specific services can defined within them using values above 100. + +uint8 RESULT_FEATURE_UNSUPPORTED = 0 # Feature is not supported by the simulator, check GetSimulatorFeatures. + # While feature support can sometimes be deduced from presence of corresponding + # service / action interface, in other cases it is about supporting certain + # call parameters, formats and options within interface call. +uint8 RESULT_OK = 1 +uint8 RESULT_NOT_FOUND = 2 # No match for input (such as when entity name does not exist). +uint8 RESULT_INCORRECT_STATE = 3 # Simulator is in an incorrect state for this interface call, e.g. a service + # requires paused state but the simulator is not paused. +uint8 RESULT_OPERATION_FAILED = 4 # Request could not be completed successfully even though feature is supported + # and the input is correct; check error_message for details. + # Implementation rule: check extended codes for called service + # (e.g. SpawnEntity) to provide a return code which is more specific. + +uint8 result # Result to be checked on return from service interface call +string error_message # Additional error description when useful. diff --git a/msg/SimulationState.msg b/msg/SimulationState.msg new file mode 100644 index 0000000..41e3942 --- /dev/null +++ b/msg/SimulationState.msg @@ -0,0 +1,16 @@ +# Simulation states used in SetSimulationState and returned in GetSimulationState + +uint8 STATE_STOPPED = 0 # Simulation is stopped, which is equivalent to pausing and resetting with ALL. + # This is typically the default state when simulator is launched. + # Stopped simulation can be played. It can also be paused, which means + # starting simulation in a paused state immediately, + # without any time steps for physics or simulated clock ticks. +uint8 STATE_PLAYING = 1 # Simulation is playing, can be either paused or stopped. +uint8 STATE_PAUSED = 2 # Simulation is paused, can be either stopped (which will reset it) or played. +uint8 STATE_QUITTING = 3 # Closing the simulator application. Switching from STATE_PLAYING or STATE_PAUSED + # states is expected to stop the simulation first, and then exit. + # Simulation interfaces will become unavailable after quitting. + # Running simulation application is outside of the simulation interfaces as + # there is no service to handle the call when the simulator is not up. + +uint8 state diff --git a/msg/SimulatorFeatures.msg b/msg/SimulatorFeatures.msg new file mode 100644 index 0000000..023c80d --- /dev/null +++ b/msg/SimulatorFeatures.msg @@ -0,0 +1,45 @@ +# Features supported by the simulator. + +uint8 SPAWNING = 0 # Supports spawn interface (SpawnEntity). +uint8 DELETING = 1 # Supports deleting entities (DeleteEntity). +uint8 NAMED_POSES = 2 # Supports predefined named poses (GetNamedPoses). +uint8 POSE_BOUNDS = 3 # Supports pose bounds (GetNamedPoseBounds). +uint8 ENTITY_TAGS = 4 # Supports entity tags in interfaces using EntityFilters, such as GetEntities. +uint8 ENTITY_BOUNDS = 5 # Supports entity bounds (GetEntityBounds). +uint8 ENTITY_BOUNDS_BOX = 6 # Supports entity filtering with bounds with TYPE_BOX. +uint8 ENTITY_BOUNDS_CONVEX = 7 # Supports entity filtering with Bounds TYPE_CONVEX_HULL. +uint8 ENTITY_CATEGORIES = 8 # Supports entity categories, such as in use with EntityFilters or SetEntityInfo. +uint8 SPAWNING_RESOURCE_STRING = 9 # Supports SpawnEntity resource_string field. + +uint8 ENTITY_STATE_GETTING = 10 # Supports GetEntityState interface. +uint8 ENTITY_STATE_SETTING = 11 # Supports SetEntityState interface. +uint8 ENTITY_INFO_GETTING = 12 # Supports GetEntityInfo interface. +uint8 ENTITY_INFO_SETTING = 13 # Supports SetEntityInfo interface. +uint8 SPAWNABLES = 14 # Supports GetSpawnables interface. + +uint8 SIMULATION_RESET = 20 # Supports one or more ways to reset the simulation through ResetSimulation. +uint8 SIMULATION_RESET_TIME = 21 # Supports SCOPE_TIME flag for resetting. +uint8 SIMULATION_RESET_STATE = 22 # Supports SCOPE_STATE flag for resetting. +uint8 SIMULATION_RESET_SPAWNED = 23 # Supports SCOPE_SPAWNED flag for resetting. +uint8 SIMULATION_STATE_GETTING = 24 # Supports GetSimulationState interface. +uint8 SIMULATION_STATE_SETTING = 25 # Supports SetSimulationState interface. Check SIMULATION_STATE_PAUSE feature + # for setting STATE_PAUSED. +uint8 SIMULATION_STATE_PAUSE = 26 # Supports the STATE_PAUSED SimulationState in SetSimulationState interface. + +uint8 STEP_SIMULATION_SINGLE = 31 # Supports single stepping through simulation with StepSimulation interface. +uint8 STEP_SIMULATION_MULTIPLE = 32 # Supports multi-stepping through simulation, either through StepSimulation. + # service or through SimulateSteps action. +uint8 STEP_SIMULATION_ACTION = 33 # Supports SimulateSteps action interface. + + +uint16[] features # A list of simulation features as specified by the list above. + +# A list of additional supported formats for spawning, which might be empty. Values may include +# * sdf (SDFormat) +# * urdf (Unified Robot Description Format) +# * usd (Universal Scene Description) +# * mjcf (MuJoCo's XML format) +# or whatever simulator-native formats that are supported. +string[] spawn_formats +string custom_info # Optional: extra information for the caller, which could point to + # documentation, version compatibility and other useful meta information. diff --git a/msg/Spawnable.msg b/msg/Spawnable.msg new file mode 100644 index 0000000..c89eae2 --- /dev/null +++ b/msg/Spawnable.msg @@ -0,0 +1,5 @@ +# Robot or other object which can be spawned in simulation runtime. + +string uri # URI which will be accepted by SpawnEntity service. +string description # Optional description for the user, e.g. "robot X with sensors A,B,C". +Bounds spawn_bounds # Optional spawn area bounds which fully encompass this object. diff --git a/msg/TagsFilter.msg b/msg/TagsFilter.msg new file mode 100644 index 0000000..5d087f7 --- /dev/null +++ b/msg/TagsFilter.msg @@ -0,0 +1,10 @@ +# An utility message type for specification of tag-based filtering + +string[] tags # Optional, defaults to empty, which means no tags filter. + # Results matching any or all of tags will be returned, depending + # on tags_filter_mode. + +uint8 FILTER_MODE_ANY = 0 # Filter with OR mode (any tag can match). +uint8 FILTER_MODE_ALL = 1 # Filter with AND mode (all tags need to match). + +uint8 filter_mode # Set to control filter application for tags. diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..a3b3729 --- /dev/null +++ b/package.xml @@ -0,0 +1,28 @@ + + + + simulation_interfaces + 1.0.0 + A package containing simulation interfaces including messages, services and actions + Adam Dabrowski + Apache License 2.0 + https://github.com/ros-simulation/simulation-interfaces + Adam Dabrowski + + ament_cmake + rosidl_default_generators + + builtin_interfaces + geometry_msgs + std_msgs + + rosidl_default_runtime + + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/srv/DeleteEntity.srv b/srv/DeleteEntity.srv new file mode 100644 index 0000000..334c92b --- /dev/null +++ b/srv/DeleteEntity.srv @@ -0,0 +1,9 @@ +# Remove an entity (a robot, other object) from the simulation. +# Support for this interface is indicated through the DELETING value in GetSimulationFeatures. + +string entity # Entity identified by its unique name with a namespace, + # as returned by SpawnEntity or GetEntities. + +--- + +Result result diff --git a/srv/GetEntities.srv b/srv/GetEntities.srv new file mode 100644 index 0000000..3cbf707 --- /dev/null +++ b/srv/GetEntities.srv @@ -0,0 +1,11 @@ +# Get objects in the scene which can be interacted with, e.g. with using SetEntityState. +# You can get further information about entities through GetEntityInfo and GetEntityState services. +# There is also a GetEntitiesStates service if you would like to get state for each entity. + +EntityFilters filters # Optional filters for the query, including name, category, tags, + # and overlap filters. + +--- + +Result result +string[] entities # Unique names of all entities matching the filters. diff --git a/srv/GetEntitiesStates.srv b/srv/GetEntitiesStates.srv new file mode 100644 index 0000000..1e18846 --- /dev/null +++ b/srv/GetEntitiesStates.srv @@ -0,0 +1,12 @@ +# Get objects in the scene which can be interacted, e.g. with using SetEntityState. +# Use GetEntities service instead if EntityState information for all entities is not needed. +# Support for this interface is indicated through the ENTITY_STATE_GETTING value in GetSimulationFeatures. + +EntityFilters filters # Optional filters for the query, including name, category, tags, + # and overlap filters. + +--- + +Result result +string[] entities # Unique names of all entities matching the filters. +EntityState[] states # States for these entities. diff --git a/srv/GetEntityBounds.srv b/srv/GetEntityBounds.srv new file mode 100644 index 0000000..3c36640 --- /dev/null +++ b/srv/GetEntityBounds.srv @@ -0,0 +1,11 @@ +# Get geometrical bounds for entity. This feature is available if GetSimulatorFeatures includes ENTITY_BOUNDS feature. +# Support for this interface is indicated through the ENTITY_BOUNDS value in GetSimulationFeatures. + +string entity # Entity identified by its unique name as returned by GetEntities / SpawnEntity. + +--- + +Result result +Bounds bounds # Entity bounds. Only valid if result.result_code is OK. + # Bounds with TYPE_BOX should be returned, unless there is a configuration + # parameter for the simulator to control the type and it is set otherwise. diff --git a/srv/GetEntityInfo.srv b/srv/GetEntityInfo.srv new file mode 100644 index 0000000..09370bc --- /dev/null +++ b/srv/GetEntityInfo.srv @@ -0,0 +1,9 @@ +# Get type and other information about an entity. +# Support for this interface is indicated through the ENTITY_INFO_GETTING value in GetSimulationFeatures. + +string entity # Entity identified by its unique name as returned by GetEntities. + +--- + +Result result +EntityInfo info # Only valid if result.result_code is OK. diff --git a/srv/GetEntityState.srv b/srv/GetEntityState.srv new file mode 100644 index 0000000..b054ec2 --- /dev/null +++ b/srv/GetEntityState.srv @@ -0,0 +1,9 @@ +# Get state of an entity. +# Support for this interface is indicated through the ENTITY_STATE_GETTING value in GetSimulationFeatures. + +string entity # Entity identified by its unique name as returned by GetEntities / SpawnEntity. + +--- + +Result result +EntityState state # Entity ground truth state. Valid when result.result = OK. diff --git a/srv/GetNamedPoseBounds.srv b/srv/GetNamedPoseBounds.srv new file mode 100644 index 0000000..088b9d3 --- /dev/null +++ b/srv/GetNamedPoseBounds.srv @@ -0,0 +1,9 @@ +# Get bounds for the named pose. This feature is available if GetSimulatorFeatures includes POSE_BOUNDS feature. +# Support for this interface is indicated through the POSE_BOUNDS value in GetSimulationFeatures. + +string name # unique name (as returned from GetNamedPoses). + +--- + +Result result +Bounds bounds # bounds for the named pose. diff --git a/srv/GetNamedPoses.srv b/srv/GetNamedPoses.srv new file mode 100644 index 0000000..cb9158c --- /dev/null +++ b/srv/GetNamedPoses.srv @@ -0,0 +1,10 @@ +# Get predefined poses which are convenient to for spawning, navigation goals etc. +# Support for this interface is indicated through the NAMED_POSES value in GetSimulationFeatures. + +TagsFilter tags # Tags filter to apply. Only named poses with matching tags field + # will be returned. Can be empty (see TagsFilter). + +--- + +Result result +NamedPose[] poses # A list of predefined poses, which may be empty. diff --git a/srv/GetSimulationState.srv b/srv/GetSimulationState.srv new file mode 100644 index 0000000..9b4cd2c --- /dev/null +++ b/srv/GetSimulationState.srv @@ -0,0 +1,8 @@ +# Gets the simulation state (paused, playing, stopped) +# Support for this interface is indicated through the SIMULATION_STATE_GETTING value in GetSimulationFeatures. + +--- + +SimulationState state # Current state of the simulation. + +Result result diff --git a/srv/GetSimulatorFeatures.srv b/srv/GetSimulatorFeatures.srv new file mode 100644 index 0000000..785efa8 --- /dev/null +++ b/srv/GetSimulatorFeatures.srv @@ -0,0 +1,6 @@ +# Get a list of features supported by the simulator. +# Simulators adhering to the standard need to implement at least this interface. + +--- + +SimulatorFeatures features diff --git a/srv/GetSpawnables.srv b/srv/GetSpawnables.srv new file mode 100644 index 0000000..ad18d0f --- /dev/null +++ b/srv/GetSpawnables.srv @@ -0,0 +1,17 @@ +# Return a list of resources which are valid as SpawnEntity uri fields (e.g. visible to or registered in simulator). +# This interface is an optional extension and might not be implemented by your simulator, check the result_code. +# Support for this interface is indicated through the SPAWNABLES value in GetSimulationFeatures. + +string[] sources # Optional field for additional sources (local or remote) to search. + # By default, each simulator has visibility of spawnables through + # some mechanisms, e.g. a set of paths, registered assets etc. + # Since the simulator cannot possibly look everywhere, + # this field allows the user to specify additional sources. + # Unrecognized values are listed as such in the result.error_message, + # but do not hinder success of the response. + # Sources may include subcategories and be simulator-specific. + +--- + +Result result +Spawnable[] spawnables # Spawnable objects with URI and additional information. diff --git a/srv/ResetSimulation.srv b/srv/ResetSimulation.srv new file mode 100644 index 0000000..a53afd5 --- /dev/null +++ b/srv/ResetSimulation.srv @@ -0,0 +1,18 @@ +# Reset the simulation to the start, including the entire scene and the simulation time. +# Objects that were dynamically spawned are de-spawned. +# Support for this interface is indicated through the SIMULATION_RESET value in GetSimulationFeatures, +# and supported scopes are further detailed by SIMULATION_RESET_TIME, SIMULATION_RESET_STATE, and SIMULATION_RESET_SPAWNED. + +uint8 SCOPE_DEFAULT = 0 # same as SCOPE_ALL. +uint8 SCOPE_TIME = 1 # Reset simulation time to start. +uint8 SCOPE_STATE = 2 # Reset state such as poses and velocities. This may include state randomization + # if such feature is available and turned on. +uint8 SCOPE_SPAWNED = 4 # De-spawns all spawned entities. +uint8 SCOPE_ALL = 255 # Fully resets simulation to the start, as if it was closed and launched again. + +uint8 scope # Scope of the reset. Note that simulators might only support some scopes. + # This is a bit field which may be checked for each scope e.g. scope & SCOPE_TIME. + +--- + +Result result diff --git a/srv/SetEntityInfo.srv b/srv/SetEntityInfo.srv new file mode 100644 index 0000000..d77561e --- /dev/null +++ b/srv/SetEntityInfo.srv @@ -0,0 +1,9 @@ +# Set type, tags and other information about an entity. +# Support for this interface is indicated through the ENTITY_INFO_SETTING value in GetSimulationFeatures. + +string entity # Entity identified by its unique name as returned by GetEntities. +EntityInfo info # EntityInfo to set to the entity, which will override its current values. + +--- + +Result result diff --git a/srv/SetEntityState.srv b/srv/SetEntityState.srv new file mode 100644 index 0000000..51063f4 --- /dev/null +++ b/srv/SetEntityState.srv @@ -0,0 +1,12 @@ +# Set a state of an object, which will result in an instant change in its pose and/or twist. +# Support for this interface is indicated through the ENTITY_STATE_SETTING value in GetSimulationFeatures. + +string entity # Entity identified by its unique name as returned by GetEntities / SpawnEntity. +EntityState state # New state to set immediately. The timestamp in header is ignored. + # If non-zero twist or acceleration is requested for static object, the service call + # fails and RESULT_OPERATION_FAILED is returned. + # Note that the acceleration field may be ignored by simulators. + +--- + +Result result diff --git a/srv/SetSimulationState.srv b/srv/SetSimulationState.srv new file mode 100644 index 0000000..e5a9604 --- /dev/null +++ b/srv/SetSimulationState.srv @@ -0,0 +1,16 @@ +# Change the simulation state +# Support for this interface is indicated through the SIMULATION_STATE_SETTING value in GetSimulationFeatures. +# Support for setting STATE_PAUSED is determined by SIMULATION_STATE_PAUSE feature. + +SimulationState state # Target state to set for the simulation. + +--- + +uint8 ALREADY_IN_TARGET_STATE = 101 # Additional result type for this call, which means simulation was already in + # the target state (e.g. was already stopped when STATE_STOPPED was requested). +uint8 STATE_TRANSITION_ERROR = 102 # The simulator failed to transition to the target state. This might happen + # especially with the transition to STATE_PLAYING from STATE_STOPPED. + # See result.error_message for details. +uint8 INCORRECT_TRANSITION = 103 # Incorrect transition (pausing when in stopped state). + +Result result diff --git a/srv/SpawnEntity.srv b/srv/SpawnEntity.srv new file mode 100644 index 0000000..0e76c10 --- /dev/null +++ b/srv/SpawnEntity.srv @@ -0,0 +1,48 @@ +# Spawn an entity (a robot, other object) by name or URI +# Support for this interface is indicated through the SPAWNING value in GetSimulationFeatures. + +string name # A name to give to the spawned entity. + # If empty, a name field in the uri file or resource_string will be used, + # if supported and not empty (e.g. "name" field in SDFormat, URDF). + # If the name is still empty or not unique (as determined by the simulator), + # the service returns a generated name in the entity_name response field if the + # allow_renaming field is set to true. Otherwise, the service call fails and an + # error is returned. +bool allow_renaming # Determines whether the spawning succeeds with a non-unique name. + # If it is set to true, the user should always check entity_name response field + # and use it for any further interactions. +string uri # Resource such as SDFormat, URDF, USD or MJCF file, a native prefab, etc. + # Valid URIs can be determined by calling GetSpawnables first, and you can check + # the simulator format support by reading SimulatorFeatures spawn_formats field. + # If uri field is empty, resource_string must not be empty. +string resource_string # An entity definition file passed as a string. + # Simulators may support spawning from a file generated on the fly (e.g. XACRO). + # It is supported by your simulator if GetSimulatorFeatures includes + # SPAWNING_RESOURCE_STRING feature. + # If uri field is not empty, resource_string field will be ignored. +string entity_namespace # Spawn the entity with all its interfaces under this namespace. +geometry_msgs/PoseStamped initial_pose # Initial entity pose. + # The header contains a reference frame, which defaults to global "world" frame. + # This frame must be known to the simulator, e.g. of an object spawned earlier. + # The timestamp field in the header is ignored. + +--- + +# Additional result.result_code values for this service. Check result.error_message for further details. +uint8 NAME_NOT_UNIQUE = 101 # Given name is already taken by entity and allow_renaming is false. +uint8 NAME_INVALID = 102 # Given name is invalid in the simulator (e.g. does not meet naming + # requirements such as allowed characters). This is also returned if name is + # empty and allow_renaming is false. +uint8 UNSUPPORTED_FORMAT = 103 # Format for uri or resource string is unsupported. Check supported formats + # through GetSimulatorFeatures service, in spawn_formats field. +uint8 NO_RESOURCE = 104 # Both uri and resource string are empty. +uint8 NAMESPACE_INVALID = 105 # Namespace does not meet namespace naming standards. +uint8 RESOURCE_PARSE_ERROR = 106 # Resource file or string failed to parse. +uint8 MISSING_ASSETS = 107 # At least one of resource assets (such as meshes) was not found. +uint8 UNSUPPORTED_ASSETS = 108 # At least one of resource assets (such as meshes) is not supported. +uint8 INVALID_POSE = 109 # initial_pose is invalid, such as when the quaternion is invalid or position + # exceeds simulator world bounds. + +Result result +string entity_name # Spawned entity full name, guaranteed to be unique in the simulation. + # If allow_renaming is true, it may differ from the request name field. diff --git a/srv/StepSimulation.srv b/srv/StepSimulation.srv new file mode 100644 index 0000000..6ec1376 --- /dev/null +++ b/srv/StepSimulation.srv @@ -0,0 +1,11 @@ +# Assuming the simulation is paused, simulate a finite number of steps and return to paused state. +# The service only returns once stepping is complete, which might take considerable time. +# There is an alternative in SimulateSteps action, which is worth considering for a multi-step interface. +# Support for this interface is indicated through the STEP_SIMULATION_SINGLE and STEP_SIMULATION_MULTIPLE values +# in GetSimulationFeatures, for steps = 1 and steps > 1 correspondingly. + +uint64 steps 1 # Step through the simulation loop this many times. + +--- + +Result result # Calling with simulation not paused will return RESULT_OPERATION_FAILED and error message.