From 5267ea2f974bbf7ffb48b5cd532b3a7dccf624c7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Tue, 17 Dec 2024 15:16:13 +0100 Subject: [PATCH 01/33] simulation interfaces MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- CMakeLists.txt | 46 +++++++++++++++++++++++++++++++++++++ README.md | 6 +++-- msg/Entity.msg | 5 ++++ msg/EntityState.msg | 5 ++++ msg/SpawnPose.msg | 6 +++++ msg/Spawnable.msg | 5 ++++ package.xml | 28 ++++++++++++++++++++++ srv/DeleteEntity.srv | 8 +++++++ srv/GetEntities.srv | 10 ++++++++ srv/GetEntityState.srv | 10 ++++++++ srv/GetSpawnPoses.srv | 10 ++++++++ srv/GetSpawnables.srv | 9 ++++++++ srv/ResetSimulation.srv | 7 ++++++ srv/SetEntityState.srv | 9 ++++++++ srv/SetSimulationPaused.srv | 9 ++++++++ srv/SpawnEntity.srv | 16 +++++++++++++ 16 files changed, 187 insertions(+), 2 deletions(-) create mode 100644 CMakeLists.txt create mode 100644 msg/Entity.msg create mode 100644 msg/EntityState.msg create mode 100644 msg/SpawnPose.msg create mode 100644 msg/Spawnable.msg create mode 100644 package.xml create mode 100644 srv/DeleteEntity.srv create mode 100644 srv/GetEntities.srv create mode 100644 srv/GetEntityState.srv create mode 100644 srv/GetSpawnPoses.srv create mode 100644 srv/GetSpawnables.srv create mode 100644 srv/ResetSimulation.srv create mode 100644 srv/SetEntityState.srv create mode 100644 srv/SetSimulationPaused.srv create mode 100644 srv/SpawnEntity.srv diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..a7f7f49 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.8) +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/Entity.msg" + "msg/EntityState.msg" + "msg/Spawnable.msg" + "msg/SpawnPose.msg" +) + +set(srv_files + "srv/DeleteEntity.srv" + "srv/GetEntities.srv" + "srv/GetEntityState.srv" + "srv/GetSpawnables.srv" + "srv/GetSpawnPoses.srv" + "srv/ResetSimulation.srv" + "srv/SetEntityState.srv" + "srv/SetSimulationPaused.srv" + "srv/SpawnEntity.srv" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_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..69b3514 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,4 @@ -# simulation_interfaces -Standard interfaces for interacting with simulators from ROS 2 +# Simulation Interfaces + +Standard ROS 2 interfaces for interacting with simulators. +Messages and services are documented in their respective files. diff --git a/msg/Entity.msg b/msg/Entity.msg new file mode 100644 index 0000000..3fd9243 --- /dev/null +++ b/msg/Entity.msg @@ -0,0 +1,5 @@ +# Entity ground truth state in the simulation + +string name # Entity's scoped, unique name (including namespace) +string description # Optional: additional information about entity. +EntityState state # Entity current state diff --git a/msg/EntityState.msg b/msg/EntityState.msg new file mode 100644 index 0000000..76efce0 --- /dev/null +++ b/msg/EntityState.msg @@ -0,0 +1,5 @@ +# Entity current pose and twist + +std_msgs/Header header # Frame and timestamp for pose and twist. Empty frame defaults world. +geometry_msgs/Pose pose # Pose in reference frame, ground truth. +geometry_msgs/Twist twist # Twist in reference frame, ground truth. diff --git a/msg/SpawnPose.msg b/msg/SpawnPose.msg new file mode 100644 index 0000000..ac2509e --- /dev/null +++ b/msg/SpawnPose.msg @@ -0,0 +1,6 @@ +# A named spawn pose (point) + +string point_name # Name of a spawn point. +string point_description # Description for the user, e.g. "near the charging station". +geometry_msgs/Pose spawn_pose # Spawn point pose, which can be used with SpawnEntity.srv. +geometry_msgs/Polygon bounds # Optional: limits of spawning area around pose, e.g. ground level. diff --git a/msg/Spawnable.msg b/msg/Spawnable.msg new file mode 100644 index 0000000..8a7ddb8 --- /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". +geometry_msgs/Polygon bounds # Optional: shape bounding the entity, e.g. to determine valid spawn poses. diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..a8a6589 --- /dev/null +++ b/package.xml @@ -0,0 +1,28 @@ + + + + simulation_interfaces + 1.0.0 + A package containing simulation interfaces including messages and services + 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..0a42d2c --- /dev/null +++ b/srv/DeleteEntity.srv @@ -0,0 +1,8 @@ +# Remove an entity (a robot, other object) by unique name from the simulation + +string name # Unique name with a namespace, as returned by SpawnEntity or GetEntities. + +--- + +bool success # return true if deleted successfully. +string status_message # On failure, an user-ready error message. diff --git a/srv/GetEntities.srv b/srv/GetEntities.srv new file mode 100644 index 0000000..77e5bc8 --- /dev/null +++ b/srv/GetEntities.srv @@ -0,0 +1,10 @@ +# Get objects in the scene which can be interacted, e.g. with using SetEntityState. + +string filter # Optional, defaults to empty. Return entities with matching names. + +--- + +Entity[] entities # All entities with names matching the filter, poses and twists. + +bool success # Return true if successful. +string status_message # Additional comments or error status. diff --git a/srv/GetEntityState.srv b/srv/GetEntityState.srv new file mode 100644 index 0000000..676cbb0 --- /dev/null +++ b/srv/GetEntityState.srv @@ -0,0 +1,10 @@ +# Get state of an object. Valid objects are on the list returned by GetEntities. + +string name # Unique name as returned by GetEntities / SpawnEntity. + +--- + +EntityState entity_state # Entity ground truth state including pose and twist. + +bool success # Return true if successful. +string status_message # Additional comments or error status. diff --git a/srv/GetSpawnPoses.srv b/srv/GetSpawnPoses.srv new file mode 100644 index 0000000..b7d03cf --- /dev/null +++ b/srv/GetSpawnPoses.srv @@ -0,0 +1,10 @@ +# Get predefined spawn poses which is convenient to avoid spawning in invalid spaces. +# This is an optional simulation feature: check status_message if you receive an empty spawn_poses vector, +# to determine whether this feature is not supported by your simulator, or spawn poses are simply not defined. + +--- + +SpawnPose[] spawn_poses # A list of predefined spawn_poses, which may be empty. + +bool success # Return true if successful. +string status_message # Additional comments or error status. diff --git a/srv/GetSpawnables.srv b/srv/GetSpawnables.srv new file mode 100644 index 0000000..882b4be --- /dev/null +++ b/srv/GetSpawnables.srv @@ -0,0 +1,9 @@ +# 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 status_message. + +--- + +Spawnable[] spawnables # Spawnable objects with URI and additional information. + +bool success # Return true if successful. +string status_message # Additional comments or error status. diff --git a/srv/ResetSimulation.srv b/srv/ResetSimulation.srv new file mode 100644 index 0000000..733c5ba --- /dev/null +++ b/srv/ResetSimulation.srv @@ -0,0 +1,7 @@ +# Reset the simulation to the start, including the entire scene and the simulation time. +# Objects that were dynamically spawned are de-spawned. + +--- + +bool success # Return true if successful. +string status_message # Additional comments or error status.. diff --git a/srv/SetEntityState.srv b/srv/SetEntityState.srv new file mode 100644 index 0000000..30907c6 --- /dev/null +++ b/srv/SetEntityState.srv @@ -0,0 +1,9 @@ +# Set a state of an object, which will result in an instant change in its pose and/or twist. + +string name # Unique name as returned by GetEntities or SpawnEntity. +EntityState state # New state to set immediately. The timestamp in header is ignored. + +--- + +bool success # Return true if successful. +string status_message # Additional comments or error status. diff --git a/srv/SetSimulationPaused.srv b/srv/SetSimulationPaused.srv new file mode 100644 index 0000000..8b9270c --- /dev/null +++ b/srv/SetSimulationPaused.srv @@ -0,0 +1,9 @@ +# Pauses or unpauses the simulation + +bool pause # If true, sets simulation state to paused, otherwise sets it to running. + # If already in target state, nothing happens but status_message informs about it. + +--- + +bool success # Return true if pausing / resuming was successful. +string status_message # Additional comments or error status.. diff --git a/srv/SpawnEntity.srv b/srv/SpawnEntity.srv new file mode 100644 index 0000000..55325e6 --- /dev/null +++ b/srv/SpawnEntity.srv @@ -0,0 +1,16 @@ +# Spawn an entity (a robot, other object) by name or URI + +string name # Suggest a name to give to the spawned entity. If left empty, it will be generated. + # Due to requirement for name uniqueness and the namespace, check the status_message + # to get an actual unique name to use for further queries. +string uri # Resource such as SDF or URDF file, or in native format such as prefab. + # Valid URIs can be determined by calling GetSpawnables first, + # if the feature is supported by your simulator. +string robot_namespace # Spawn robot or another object with all its interfaces under this namespace. +string reference_frame # Optional initial pose reference frame. Defaults to global "world" frame. +geometry_msgs/Pose initial_pose # Initial entity pose. + +--- + +bool success # Return true if spawned successfully. +string status_message # Spawned entity full name (with namespace) on success or user-ready error message. From 0cfd0fdeed18312a6cc8b165cd9a2b1b7692c31c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Thu, 19 Dec 2024 15:32:14 +0100 Subject: [PATCH 02/33] Apply suggestions from code review Co-authored-by: Martin Pecka Co-authored-by: Steve Peters --- msg/Entity.msg | 2 +- msg/EntityState.msg | 2 +- srv/DeleteEntity.srv | 2 +- srv/ResetSimulation.srv | 2 +- srv/SetSimulationPaused.srv | 2 +- srv/SpawnEntity.srv | 3 ++- 6 files changed, 7 insertions(+), 6 deletions(-) diff --git a/msg/Entity.msg b/msg/Entity.msg index 3fd9243..9119c6d 100644 --- a/msg/Entity.msg +++ b/msg/Entity.msg @@ -1,5 +1,5 @@ # Entity ground truth state in the simulation -string name # Entity's scoped, unique name (including namespace) +string name # Scoped name of the entity. It must be unique (including namespace) string description # Optional: additional information about entity. EntityState state # Entity current state diff --git a/msg/EntityState.msg b/msg/EntityState.msg index 76efce0..e017abe 100644 --- a/msg/EntityState.msg +++ b/msg/EntityState.msg @@ -1,5 +1,5 @@ # Entity current pose and twist -std_msgs/Header header # Frame and timestamp for pose and twist. Empty frame defaults world. +std_msgs/Header header # 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 # Twist in reference frame, ground truth. diff --git a/srv/DeleteEntity.srv b/srv/DeleteEntity.srv index 0a42d2c..a8f1200 100644 --- a/srv/DeleteEntity.srv +++ b/srv/DeleteEntity.srv @@ -5,4 +5,4 @@ string name # Unique name with a namespace, as returned --- bool success # return true if deleted successfully. -string status_message # On failure, an user-ready error message. +string status_message # On failure, a user-friendly error message. diff --git a/srv/ResetSimulation.srv b/srv/ResetSimulation.srv index 733c5ba..0508217 100644 --- a/srv/ResetSimulation.srv +++ b/srv/ResetSimulation.srv @@ -4,4 +4,4 @@ --- bool success # Return true if successful. -string status_message # Additional comments or error status.. +string status_message # Additional comments or error status. diff --git a/srv/SetSimulationPaused.srv b/srv/SetSimulationPaused.srv index 8b9270c..52aea70 100644 --- a/srv/SetSimulationPaused.srv +++ b/srv/SetSimulationPaused.srv @@ -6,4 +6,4 @@ bool pause # If true, sets simulation state to paused, --- bool success # Return true if pausing / resuming was successful. -string status_message # Additional comments or error status.. +string status_message # Additional comments or error status. diff --git a/srv/SpawnEntity.srv b/srv/SpawnEntity.srv index 55325e6..bcabfe3 100644 --- a/srv/SpawnEntity.srv +++ b/srv/SpawnEntity.srv @@ -13,4 +13,5 @@ geometry_msgs/Pose initial_pose # Initial entity pose. --- bool success # Return true if spawned successfully. -string status_message # Spawned entity full name (with namespace) on success or user-ready error message. +string status_message # On failure, a user-friendly error message. +string unique_name # Spawned entity full name (with namespace). From d874c2392eb326acabe33e80a03e8b97f52d8a68 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Thu, 19 Dec 2024 15:33:10 +0100 Subject: [PATCH 03/33] applied review suggestions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- msg/Entity.msg | 5 ++--- msg/EntityState.msg | 1 + msg/EntityWithState.msg | 4 ++++ msg/SpawnPose.msg | 5 +++-- msg/Spawnable.msg | 4 ++-- msg/SpawnableBounds.msg | 5 +++++ srv/GetEntities.srv | 4 +++- srv/GetEntityState.srv | 2 +- srv/ResetSimulation.srv | 10 +++++++++- srv/SpawnEntity.srv | 4 +++- 10 files changed, 33 insertions(+), 11 deletions(-) create mode 100644 msg/EntityWithState.msg create mode 100644 msg/SpawnableBounds.msg diff --git a/msg/Entity.msg b/msg/Entity.msg index 9119c6d..f1718e8 100644 --- a/msg/Entity.msg +++ b/msg/Entity.msg @@ -1,5 +1,4 @@ -# Entity ground truth state in the simulation +# Entity name and description -string name # Scoped name of the entity. It must be unique (including namespace) +string name # Scoped name of the entity. It must be unique (including namespace). string description # Optional: additional information about entity. -EntityState state # Entity current state diff --git a/msg/EntityState.msg b/msg/EntityState.msg index e017abe..a271cf9 100644 --- a/msg/EntityState.msg +++ b/msg/EntityState.msg @@ -3,3 +3,4 @@ std_msgs/Header header # 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 # Twist in reference frame, ground truth. +geometry_msgs/Accel acceleration # Linear and angular acceleration ground truth. \ No newline at end of file diff --git a/msg/EntityWithState.msg b/msg/EntityWithState.msg new file mode 100644 index 0000000..782482a --- /dev/null +++ b/msg/EntityWithState.msg @@ -0,0 +1,4 @@ +# Entity with its current ground truth state + +Entity entity # Entity name and description +EntityState state # Entity current state diff --git a/msg/SpawnPose.msg b/msg/SpawnPose.msg index ac2509e..0ae5a00 100644 --- a/msg/SpawnPose.msg +++ b/msg/SpawnPose.msg @@ -1,6 +1,7 @@ # A named spawn pose (point) -string point_name # Name of a spawn point. +string point_name # Name of a spawn point. It does not need to be unique. string point_description # Description for the user, e.g. "near the charging station". geometry_msgs/Pose spawn_pose # Spawn point pose, which can be used with SpawnEntity.srv. -geometry_msgs/Polygon bounds # Optional: limits of spawning area around pose, e.g. ground level. + +SpawnableBounds spawn_bounds # Optional spawn area bounds. \ No newline at end of file diff --git a/msg/Spawnable.msg b/msg/Spawnable.msg index 8a7ddb8..dbd92bc 100644 --- a/msg/Spawnable.msg +++ b/msg/Spawnable.msg @@ -1,5 +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". -geometry_msgs/Polygon bounds # Optional: shape bounding the entity, e.g. to determine valid spawn poses. +string description # Optional description for the user, e.g. "robot X with sensors A,B,C". +SpawnableBounds spawn_bounds # Optional spawn area bounds which fully encompass this object. diff --git a/msg/SpawnableBounds.msg b/msg/SpawnableBounds.msg new file mode 100644 index 0000000..83a554e --- /dev/null +++ b/msg/SpawnableBounds.msg @@ -0,0 +1,5 @@ +# Bounds for spawning an entity, e.g. to avoid spawning with other object overlap. +# Spawn pose might be valid for a small object, but not suitable for a large one, or a differently shaped one. +# These limits are relative to spawn pose or entity's top link transform, following ROS rep-103 conventions. + +geometry_msgs/Vector3 bounding_box # Optional: limits specified as a non-aa box with spawn_pose in the middle. \ No newline at end of file diff --git a/srv/GetEntities.srv b/srv/GetEntities.srv index 77e5bc8..ba8350d 100644 --- a/srv/GetEntities.srv +++ b/srv/GetEntities.srv @@ -1,10 +1,12 @@ # Get objects in the scene which can be interacted, e.g. with using SetEntityState. string filter # Optional, defaults to empty. Return entities with matching names. + # Entities containing a case-sensitive filter substring are returned. + # An empty filter will result in all entities being returned. --- -Entity[] entities # All entities with names matching the filter, poses and twists. +EntityWithState[] entities # All entities with their states, matching the filter. bool success # Return true if successful. string status_message # Additional comments or error status. diff --git a/srv/GetEntityState.srv b/srv/GetEntityState.srv index 676cbb0..314a69c 100644 --- a/srv/GetEntityState.srv +++ b/srv/GetEntityState.srv @@ -4,7 +4,7 @@ string name # Unique name as returne --- -EntityState entity_state # Entity ground truth state including pose and twist. +EntityState entity_state # Entity ground truth state. bool success # Return true if successful. string status_message # Additional comments or error status. diff --git a/srv/ResetSimulation.srv b/srv/ResetSimulation.srv index 0508217..c83a92d 100644 --- a/srv/ResetSimulation.srv +++ b/srv/ResetSimulation.srv @@ -1,7 +1,15 @@ # Reset the simulation to the start, including the entire scene and the simulation time. # Objects that were dynamically spawned are de-spawned. +uint8 DEFAULT=0 # same as ALL. +uint8 TIME=1 # Reset simulation time to start. +uint8 OBJECTS=2 # Reset all poses and velocities. +uint8 SPAWNED=4 # De-spawns all spawned entities. +uint8 ALL=7 # Fully resets simulation to the start, as if it was closed and launched again. + +uint8 reset_scope # Scope of the reset. Note that simulators might only support some types of resets. + --- bool success # Return true if successful. -string status_message # Additional comments or error status. +string status_message # Additional comments or error status, such as what reset types are supported. diff --git a/srv/SpawnEntity.srv b/srv/SpawnEntity.srv index bcabfe3..8173e96 100644 --- a/srv/SpawnEntity.srv +++ b/srv/SpawnEntity.srv @@ -8,7 +8,9 @@ string uri # Resource such as SDF or URDF file, or in n # if the feature is supported by your simulator. string robot_namespace # Spawn robot or another object with all its interfaces under this namespace. string reference_frame # Optional initial pose reference frame. Defaults to global "world" frame. -geometry_msgs/Pose initial_pose # Initial entity pose. + # This ROS frame must be known to the simulator, e.g. of an object spawned earlier. +geometry_msgs/Pose initial_pose # Initial entity pose. Simulators should spawn the entity with its top-level link + # transform matching this pose. --- From 10f59d251dd73ff070311933e00c539c963b358d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Thu, 19 Dec 2024 15:38:29 +0100 Subject: [PATCH 04/33] added missing cmake entries MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index a7f7f49..afbe138 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,7 +18,9 @@ find_package(std_msgs REQUIRED) set(msg_files "msg/Entity.msg" "msg/EntityState.msg" + "msg/EntityWithState.msg" "msg/Spawnable.msg" + "msg/SpawnableBounds.msg" "msg/SpawnPose.msg" ) From ea23f598f1d1ac74996a38b89b11d3592fa4eb0b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Thu, 19 Dec 2024 16:07:34 +0100 Subject: [PATCH 05/33] stepping service MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- CMakeLists.txt | 1 + srv/StepSimulation.srv | 10 ++++++++++ 2 files changed, 11 insertions(+) create mode 100644 srv/StepSimulation.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index afbe138..c02ee5f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,6 +34,7 @@ set(srv_files "srv/SetEntityState.srv" "srv/SetSimulationPaused.srv" "srv/SpawnEntity.srv" + "srv/StepSimulation.srv" ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/srv/StepSimulation.srv b/srv/StepSimulation.srv new file mode 100644 index 0000000..c29442c --- /dev/null +++ b/srv/StepSimulation.srv @@ -0,0 +1,10 @@ +# If simulation is paused, take N steps and pause it back. The service only returns once stepping is complete, which +# might take considerable time. + +uint16 steps # Step through the simulation loop this many times. + +--- + +bool success # Return true if stepping is supported and was successful. +string status_message # Additional comments or error status. + # Calling with simulation not paused will return failure and an appropriate message. From 97b79f764ec9a81be09dfa9e23b7ae8948c1aabb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Thu, 19 Dec 2024 16:11:33 +0100 Subject: [PATCH 06/33] robot_namespace -> namespace MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski robot_namespace -> namespace Signed-off-by: Adam Dąbrowski --- srv/SpawnEntity.srv | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/srv/SpawnEntity.srv b/srv/SpawnEntity.srv index 8173e96..660a4c8 100644 --- a/srv/SpawnEntity.srv +++ b/srv/SpawnEntity.srv @@ -6,7 +6,7 @@ string name # Suggest a name to give to the spawned enti string uri # Resource such as SDF or URDF file, or in native format such as prefab. # Valid URIs can be determined by calling GetSpawnables first, # if the feature is supported by your simulator. -string robot_namespace # Spawn robot or another object with all its interfaces under this namespace. +string entity_namespace # Spawn robot or another object with all its interfaces under this namespace. string reference_frame # Optional initial pose reference frame. Defaults to global "world" frame. # This ROS frame must be known to the simulator, e.g. of an object spawned earlier. geometry_msgs/Pose initial_pose # Initial entity pose. Simulators should spawn the entity with its top-level link From 8bae938b96933e6d64185bac6aa6b3fb71e464df Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Thu, 19 Dec 2024 16:54:39 +0100 Subject: [PATCH 07/33] changed filter semantics to regex MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- srv/GetEntities.srv | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/srv/GetEntities.srv b/srv/GetEntities.srv index ba8350d..2a7e0a7 100644 --- a/srv/GetEntities.srv +++ b/srv/GetEntities.srv @@ -1,7 +1,7 @@ # Get objects in the scene which can be interacted, e.g. with using SetEntityState. string filter # Optional, defaults to empty. Return entities with matching names. - # Entities containing a case-sensitive filter substring are returned. + # Entity names are matched with the filter regular expression. # An empty filter will result in all entities being returned. --- From 885a3fa317b63b6bcf77235755b6d6d4c0228d5f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Tue, 7 Jan 2025 12:30:04 +0100 Subject: [PATCH 08/33] Updated EntityState and SpawnableBounds messages to address feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- msg/EntityState.msg | 6 ++++-- msg/SpawnableBounds.msg | 8 +++++++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/msg/EntityState.msg b/msg/EntityState.msg index a271cf9..ffea791 100644 --- a/msg/EntityState.msg +++ b/msg/EntityState.msg @@ -2,5 +2,7 @@ std_msgs/Header header # 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 # Twist in reference frame, ground truth. -geometry_msgs/Accel acceleration # Linear and angular acceleration ground truth. \ No newline at end of file +geometry_msgs/Twist twist # Ground truth body twist in the reference frame. + # It is observed and defined in the local coordinates system of the body. + # 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. \ No newline at end of file diff --git a/msg/SpawnableBounds.msg b/msg/SpawnableBounds.msg index 83a554e..dd516c4 100644 --- a/msg/SpawnableBounds.msg +++ b/msg/SpawnableBounds.msg @@ -2,4 +2,10 @@ # Spawn pose might be valid for a small object, but not suitable for a large one, or a differently shaped one. # These limits are relative to spawn pose or entity's top link transform, following ROS rep-103 conventions. -geometry_msgs/Vector3 bounding_box # Optional: limits specified as a non-aa box with spawn_pose in the middle. \ No newline at end of file +# By default, both fields are zero vectors which means that spawning is unbounded. +# Otherwise, the fields are expected to form a valid box containing the (0,0,0) point which is the spawn position. +# You should check whether the entity simulation model fits within the bounds before spawning, to avoid overlaps and +# unstable behavior. + +geometry_msgs/Vector3 upper_right # Optional: the upper right corner of the spawn box. +geometry_msgs/Vector3 lower_left # Optional: the lower left corner of the spawn box. From 53d6ee988a582b6d38196a2923c1f3433611c023 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Tue, 7 Jan 2025 14:24:53 +0100 Subject: [PATCH 09/33] Added action for multi-stepping MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- CMakeLists.txt | 5 +++++ action/MultiStepSimulation.action | 9 +++++++++ 2 files changed, 14 insertions(+) create mode 100644 action/MultiStepSimulation.action diff --git a/CMakeLists.txt b/CMakeLists.txt index c02ee5f..ac7f4fe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,9 +37,14 @@ set(srv_files "srv/StepSimulation.srv" ) +set(action_files + "action/MultiStepSimulation.action" +) + rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} ${srv_files} + ${action_files} DEPENDENCIES builtin_interfaces std_msgs geometry_msgs ADD_LINTER_TESTS ) diff --git a/action/MultiStepSimulation.action b/action/MultiStepSimulation.action new file mode 100644 index 0000000..dfcaaec --- /dev/null +++ b/action/MultiStepSimulation.action @@ -0,0 +1,9 @@ +# If simulation is paused, take N steps and pause it back. The action returns feedback after each complete step. + +uint16 steps # Action goal: step through the simulation loop this many times. +--- +bool success # Return true if stepping is supported and was successful. +string status_message # Additional comments or error status. + # Calling with simulation not paused will return failure and an appropriate message. +--- +uint16 completed_steps # Action feedback: number of completed steps so far. From aaee904a3e5ffc382b8efe8ac207dc7b60c7a9d6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Tue, 7 Jan 2025 14:30:47 +0100 Subject: [PATCH 10/33] Added stepping alternatives documentation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- action/MultiStepSimulation.action | 1 + srv/StepSimulation.srv | 1 + 2 files changed, 2 insertions(+) diff --git a/action/MultiStepSimulation.action b/action/MultiStepSimulation.action index dfcaaec..379e9fa 100644 --- a/action/MultiStepSimulation.action +++ b/action/MultiStepSimulation.action @@ -1,4 +1,5 @@ # If simulation is paused, take N steps and pause it back. 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). uint16 steps # Action goal: step through the simulation loop this many times. --- diff --git a/srv/StepSimulation.srv b/srv/StepSimulation.srv index c29442c..2001f45 100644 --- a/srv/StepSimulation.srv +++ b/srv/StepSimulation.srv @@ -1,5 +1,6 @@ # If simulation is paused, take N steps and pause it back. The service only returns once stepping is complete, which # might take considerable time. +# There is an alternative in MultiStepSimulation action, which is worth considering for a multi-step interface. uint16 steps # Step through the simulation loop this many times. From df98b24791a527bb50d9964d6cb746489f6655b1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Tue, 7 Jan 2025 15:09:37 +0100 Subject: [PATCH 11/33] Added GetSimulatorFeatures interface MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- CMakeLists.txt | 1 + srv/GetSimulatorFeatures.srv | 32 ++++++++++++++++++++++++++++++++ 2 files changed, 33 insertions(+) create mode 100644 srv/GetSimulatorFeatures.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index ac7f4fe..0146432 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,6 +28,7 @@ set(srv_files "srv/DeleteEntity.srv" "srv/GetEntities.srv" "srv/GetEntityState.srv" + "srv/GetSimulatorFeatures.srv" "srv/GetSpawnables.srv" "srv/GetSpawnPoses.srv" "srv/ResetSimulation.srv" diff --git a/srv/GetSimulatorFeatures.srv b/srv/GetSimulatorFeatures.srv new file mode 100644 index 0000000..596058f --- /dev/null +++ b/srv/GetSimulatorFeatures.srv @@ -0,0 +1,32 @@ +# Get a list of features supported by the simulator. +# Simulators adhering to the standard need to implement at least this interface. + +--- + +uint8 SPAWNING = 0 # Supports spawn interface (SpawnEntity). +uint8 DELETING = 1 # Supports deleting entities (DeleteEntity). +uint8 SPAWN_POSES = 2 # Supports predefined spawn poses (GetSpawnPoses). +uint8 SPAWN_BOUNDS = 3 # Supports spawn bounds (SpawnableBounds). +uint8 SPAWN_FORMAT_SDF = 4 # Supports spawning from SDFormat specified in the uri field of SpawnEntity. +uint8 SPAWN_FORMAT_URDF = 5 +uint8 SPAWN_FORMAT_USD = 6 +uint8 SPAWN_FORMAT_NATIVE = 7 # Supports spawning from prefab / other simulator native format. + +uint8 ENTITY_STATE_LISTING = 8 # Supports GetEntityState interface. +uint8 ENTITY_STATE_SETTING = 9 # Supports SetEntityState interface. + +uint8 SIMULATION_RESET = 10 # Supports one or more ways to reset the simulation through ResetSimulation. +uint8 SIMULATION_RESET_TIME = 11 # Supports TIME flag for resetting. +uint8 SIMULATION_RESET_OBJECTS = 12 # Supports OBJECTS flag for resetting. +uint8 SIMULATION_RESET_SPAWNED = 13 # Supports SPAWNED flag for resetting. + +uint8 SIMULATION_PAUSE = 14 # Supports SetSimulationPaused interface +uint8 STEP_SIMULATION_SINGLE = 15 # Supports single stepping through simulation with StepSimulation interface. +uint8 STEP_SIMULATION_MULTIPLE = 16 # Supports multi-stepping through simulation, either through StepSimulation. + # service or through MultiStepSimulation action. +uint8 STEP_SIMULATION_ACTION = 17 # Supports MultiStepSimulation action interface. + + +uint8[] features # A list of simulation features as specified by the list above. +string custom_info # Optional: additional information for the caller, which could point to documentation, + # version compatibility and other useful meta information. From 4d551f1850d6435e4b13d0d063d6fa0dcffa8a64 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Mon, 13 Jan 2025 13:39:57 +0100 Subject: [PATCH 12/33] Apply suggestions from code review Co-authored-by: David V. Lu!! --- action/MultiStepSimulation.action | 5 +++-- msg/EntityState.msg | 2 +- msg/SpawnPose.msg | 10 +++++----- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/action/MultiStepSimulation.action b/action/MultiStepSimulation.action index 379e9fa..0ddd69c 100644 --- a/action/MultiStepSimulation.action +++ b/action/MultiStepSimulation.action @@ -1,4 +1,5 @@ -# If simulation is paused, take N steps and pause it back. The action returns feedback after each complete step. +# 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). uint16 steps # Action goal: step through the simulation loop this many times. @@ -7,4 +8,4 @@ bool success # Return true if stepping is supported and w string status_message # Additional comments or error status. # Calling with simulation not paused will return failure and an appropriate message. --- -uint16 completed_steps # Action feedback: number of completed steps so far. +uint16 completed_steps # Action feedback: number of completed steps in this action so far. diff --git a/msg/EntityState.msg b/msg/EntityState.msg index ffea791..7434cb0 100644 --- a/msg/EntityState.msg +++ b/msg/EntityState.msg @@ -5,4 +5,4 @@ geometry_msgs/Pose pose # Pose in reference frame, ground truth. geometry_msgs/Twist twist # Ground truth body twist in the reference frame. # It is observed and defined in the local coordinates system of the body. # 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. \ No newline at end of file +geometry_msgs/Accel acceleration # Linear and angular acceleration ground truth, following the same convention. diff --git a/msg/SpawnPose.msg b/msg/SpawnPose.msg index 0ae5a00..0a79102 100644 --- a/msg/SpawnPose.msg +++ b/msg/SpawnPose.msg @@ -1,7 +1,7 @@ -# A named spawn pose (point) +# A named spawn pose -string point_name # Name of a spawn point. It does not need to be unique. -string point_description # Description for the user, e.g. "near the charging station". -geometry_msgs/Pose spawn_pose # Spawn point pose, which can be used with SpawnEntity.srv. +string pose_name # Name of a spawn pose. It does not need to be unique. +string pose_description # Description for the user, e.g. "near the charging station". +geometry_msgs/Pose pose # Spawn pose, which can be used with SpawnEntity.srv. -SpawnableBounds spawn_bounds # Optional spawn area bounds. \ No newline at end of file +SpawnableBounds spawn_bounds # Optional spawn area bounds. From 95f46d963b94b054643c7f4ef4cca70a105981ac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Tue, 14 Jan 2025 13:58:30 +0100 Subject: [PATCH 13/33] Update srv/GetEntityState.srv Co-authored-by: David V. Lu!! --- srv/GetEntityState.srv | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/srv/GetEntityState.srv b/srv/GetEntityState.srv index 314a69c..17faae8 100644 --- a/srv/GetEntityState.srv +++ b/srv/GetEntityState.srv @@ -1,4 +1,4 @@ -# Get state of an object. Valid objects are on the list returned by GetEntities. +# Get state of an entity. string name # Unique name as returned by GetEntities / SpawnEntity. From 0169e108f45b78497bc842876b295a184bc9efd1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Tue, 14 Jan 2025 14:19:28 +0100 Subject: [PATCH 14/33] Code review improvements: - wording change for stepping service - features message - comment documentation for USD and URDF formats - custom formats list field MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- CMakeLists.txt | 1 + msg/SimulatorFeatures.msg | 30 ++++++++++++++++++++++++++++++ srv/GetSimulatorFeatures.srv | 28 +--------------------------- srv/StepSimulation.srv | 4 ++-- 4 files changed, 34 insertions(+), 29 deletions(-) create mode 100644 msg/SimulatorFeatures.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 0146432..33b9902 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,7 @@ set(msg_files "msg/Entity.msg" "msg/EntityState.msg" "msg/EntityWithState.msg" + "msg/SimulatorFeatures.msg" "msg/Spawnable.msg" "msg/SpawnableBounds.msg" "msg/SpawnPose.msg" diff --git a/msg/SimulatorFeatures.msg b/msg/SimulatorFeatures.msg new file mode 100644 index 0000000..351235d --- /dev/null +++ b/msg/SimulatorFeatures.msg @@ -0,0 +1,30 @@ +# Features supported by the simulator. + +uint8 SPAWNING = 0 # Supports spawn interface (SpawnEntity). +uint8 DELETING = 1 # Supports deleting entities (DeleteEntity). +uint8 SPAWN_POSES = 2 # Supports predefined spawn poses (GetSpawnPoses). +uint8 SPAWN_BOUNDS = 3 # Supports spawn bounds (SpawnableBounds). +uint8 SPAWN_FORMAT_SDF = 4 # Supports spawning from SDFormat specified in the uri field of SpawnEntity. +uint8 SPAWN_FORMAT_URDF = 5 # Supports spawning from Unified Robot Description Format. +uint8 SPAWN_FORMAT_USD = 6 # Supports spawning from Universal Scene Description format. +uint8 SPAWN_FORMAT_NATIVE = 7 # Supports spawning from prefab / other simulator native format. + +uint8 ENTITY_STATE_LISTING = 8 # Supports GetEntityState interface. +uint8 ENTITY_STATE_SETTING = 9 # Supports SetEntityState interface. + +uint8 SIMULATION_RESET = 10 # Supports one or more ways to reset the simulation through ResetSimulation. +uint8 SIMULATION_RESET_TIME = 11 # Supports TIME flag for resetting. +uint8 SIMULATION_RESET_OBJECTS = 12 # Supports OBJECTS flag for resetting. +uint8 SIMULATION_RESET_SPAWNED = 13 # Supports SPAWNED flag for resetting. + +uint8 SIMULATION_PAUSE = 14 # Supports SetSimulationPaused interface +uint8 STEP_SIMULATION_SINGLE = 15 # Supports single stepping through simulation with StepSimulation interface. +uint8 STEP_SIMULATION_MULTIPLE = 16 # Supports multi-stepping through simulation, either through StepSimulation. + # service or through MultiStepSimulation action. +uint8 STEP_SIMULATION_ACTION = 17 # Supports MultiStepSimulation action interface. + + +uint8[] features # A list of simulation features as specified by the list above. +string[] custom_spawn_formats # A list of additional supported formats for spawning, which might be empty. +string custom_info # Optional: additional information for the caller, which could point to documentation, + # version compatibility and other useful meta information. diff --git a/srv/GetSimulatorFeatures.srv b/srv/GetSimulatorFeatures.srv index 596058f..cce072a 100644 --- a/srv/GetSimulatorFeatures.srv +++ b/srv/GetSimulatorFeatures.srv @@ -3,30 +3,4 @@ --- -uint8 SPAWNING = 0 # Supports spawn interface (SpawnEntity). -uint8 DELETING = 1 # Supports deleting entities (DeleteEntity). -uint8 SPAWN_POSES = 2 # Supports predefined spawn poses (GetSpawnPoses). -uint8 SPAWN_BOUNDS = 3 # Supports spawn bounds (SpawnableBounds). -uint8 SPAWN_FORMAT_SDF = 4 # Supports spawning from SDFormat specified in the uri field of SpawnEntity. -uint8 SPAWN_FORMAT_URDF = 5 -uint8 SPAWN_FORMAT_USD = 6 -uint8 SPAWN_FORMAT_NATIVE = 7 # Supports spawning from prefab / other simulator native format. - -uint8 ENTITY_STATE_LISTING = 8 # Supports GetEntityState interface. -uint8 ENTITY_STATE_SETTING = 9 # Supports SetEntityState interface. - -uint8 SIMULATION_RESET = 10 # Supports one or more ways to reset the simulation through ResetSimulation. -uint8 SIMULATION_RESET_TIME = 11 # Supports TIME flag for resetting. -uint8 SIMULATION_RESET_OBJECTS = 12 # Supports OBJECTS flag for resetting. -uint8 SIMULATION_RESET_SPAWNED = 13 # Supports SPAWNED flag for resetting. - -uint8 SIMULATION_PAUSE = 14 # Supports SetSimulationPaused interface -uint8 STEP_SIMULATION_SINGLE = 15 # Supports single stepping through simulation with StepSimulation interface. -uint8 STEP_SIMULATION_MULTIPLE = 16 # Supports multi-stepping through simulation, either through StepSimulation. - # service or through MultiStepSimulation action. -uint8 STEP_SIMULATION_ACTION = 17 # Supports MultiStepSimulation action interface. - - -uint8[] features # A list of simulation features as specified by the list above. -string custom_info # Optional: additional information for the caller, which could point to documentation, - # version compatibility and other useful meta information. +SimulatorFeatures simulator_features diff --git a/srv/StepSimulation.srv b/srv/StepSimulation.srv index 2001f45..5919e21 100644 --- a/srv/StepSimulation.srv +++ b/srv/StepSimulation.srv @@ -1,5 +1,5 @@ -# If simulation is paused, take N steps and pause it back. The service only returns once stepping is complete, which -# might take considerable time. +# 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 MultiStepSimulation action, which is worth considering for a multi-step interface. uint16 steps # Step through the simulation loop this many times. From 1e92784c8b2e630206426b30f09b3886a4425dad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Mon, 20 Jan 2025 10:48:12 +0100 Subject: [PATCH 15/33] Apply suggestions from code review Co-authored-by: David V. Lu!! Co-authored-by: Addisu Z. Taddese --- msg/SimulatorFeatures.msg | 33 ++++++++++++++++++--------------- msg/SpawnPose.msg | 4 ++-- srv/SpawnEntity.srv | 2 +- 3 files changed, 21 insertions(+), 18 deletions(-) diff --git a/msg/SimulatorFeatures.msg b/msg/SimulatorFeatures.msg index 351235d..0c1f8a0 100644 --- a/msg/SimulatorFeatures.msg +++ b/msg/SimulatorFeatures.msg @@ -4,27 +4,30 @@ uint8 SPAWNING = 0 # Supports spawn interface (SpawnEntity uint8 DELETING = 1 # Supports deleting entities (DeleteEntity). uint8 SPAWN_POSES = 2 # Supports predefined spawn poses (GetSpawnPoses). uint8 SPAWN_BOUNDS = 3 # Supports spawn bounds (SpawnableBounds). -uint8 SPAWN_FORMAT_SDF = 4 # Supports spawning from SDFormat specified in the uri field of SpawnEntity. -uint8 SPAWN_FORMAT_URDF = 5 # Supports spawning from Unified Robot Description Format. -uint8 SPAWN_FORMAT_USD = 6 # Supports spawning from Universal Scene Description format. -uint8 SPAWN_FORMAT_NATIVE = 7 # Supports spawning from prefab / other simulator native format. -uint8 ENTITY_STATE_LISTING = 8 # Supports GetEntityState interface. -uint8 ENTITY_STATE_SETTING = 9 # Supports SetEntityState interface. +uint8 ENTITY_STATE_LISTING = 4 # Supports GetEntityState interface. +uint8 ENTITY_STATE_SETTING = 5 # Supports SetEntityState interface. -uint8 SIMULATION_RESET = 10 # Supports one or more ways to reset the simulation through ResetSimulation. -uint8 SIMULATION_RESET_TIME = 11 # Supports TIME flag for resetting. -uint8 SIMULATION_RESET_OBJECTS = 12 # Supports OBJECTS flag for resetting. -uint8 SIMULATION_RESET_SPAWNED = 13 # Supports SPAWNED flag for resetting. +uint8 SIMULATION_RESET = 6 # Supports one or more ways to reset the simulation through ResetSimulation. +uint8 SIMULATION_RESET_TIME = 7 # Supports TIME flag for resetting. +uint8 SIMULATION_RESET_OBJECTS = 8 # Supports OBJECTS flag for resetting. +uint8 SIMULATION_RESET_SPAWNED = 9 # Supports SPAWNED flag for resetting. -uint8 SIMULATION_PAUSE = 14 # Supports SetSimulationPaused interface -uint8 STEP_SIMULATION_SINGLE = 15 # Supports single stepping through simulation with StepSimulation interface. -uint8 STEP_SIMULATION_MULTIPLE = 16 # Supports multi-stepping through simulation, either through StepSimulation. +uint8 SIMULATION_PAUSE = 10 # Supports SetSimulationPaused interface +uint8 STEP_SIMULATION_SINGLE = 11 # Supports single stepping through simulation with StepSimulation interface. +uint8 STEP_SIMULATION_MULTIPLE = 12 # Supports multi-stepping through simulation, either through StepSimulation. # service or through MultiStepSimulation action. -uint8 STEP_SIMULATION_ACTION = 17 # Supports MultiStepSimulation action interface. +uint8 STEP_SIMULATION_ACTION = 13 # Supports MultiStepSimulation action interface. uint8[] features # A list of simulation features as specified by the list above. -string[] custom_spawn_formats # A list of additional supported formats for spawning, which might be empty. + +# 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) +# or whatever simulator-native formats that are supported. +string[] spawn_formats string custom_info # Optional: additional information for the caller, which could point to documentation, # version compatibility and other useful meta information. diff --git a/msg/SpawnPose.msg b/msg/SpawnPose.msg index 0a79102..86ac0e7 100644 --- a/msg/SpawnPose.msg +++ b/msg/SpawnPose.msg @@ -1,7 +1,7 @@ # A named spawn pose string pose_name # Name of a spawn pose. It does not need to be unique. -string pose_description # Description for the user, e.g. "near the charging station". -geometry_msgs/Pose pose # Spawn pose, which can be used with SpawnEntity.srv. +string pose_description # Description for the user, e.g. "near the charging station". +geometry_msgs/Pose pose # Spawn pose relative to world, which can be used with SpawnEntity.srv. SpawnableBounds spawn_bounds # Optional spawn area bounds. diff --git a/srv/SpawnEntity.srv b/srv/SpawnEntity.srv index 660a4c8..b920483 100644 --- a/srv/SpawnEntity.srv +++ b/srv/SpawnEntity.srv @@ -6,7 +6,7 @@ string name # Suggest a name to give to the spawned enti string uri # Resource such as SDF or URDF file, or in native format such as prefab. # Valid URIs can be determined by calling GetSpawnables first, # if the feature is supported by your simulator. -string entity_namespace # Spawn robot or another object with all its interfaces under this namespace. +string entity_namespace # Spawn the entity with all its interfaces under this namespace. string reference_frame # Optional initial pose reference frame. Defaults to global "world" frame. # This ROS frame must be known to the simulator, e.g. of an object spawned earlier. geometry_msgs/Pose initial_pose # Initial entity pose. Simulators should spawn the entity with its top-level link From 9bf757fbca20b905c7ad4ec6f9d7db13eceb759b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Mon, 20 Jan 2025 12:46:34 +0100 Subject: [PATCH 16/33] Revision following the review: - Changed SpawnPose to NamedPose, added tags. - SpawnEntity interface substantially updated. - Some improved documentation, especially regarding spawning. - applied other code review comments MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- CMakeLists.txt | 6 ++-- README.md | 2 +- action/MultiStepSimulation.action | 4 +-- msg/Entity.msg | 2 +- msg/EntityState.msg | 2 +- msg/EntityWithState.msg | 3 +- msg/NamedPose.msg | 9 +++++ msg/PoseBounds.msg | 11 ++++++ msg/SimulatorFeatures.msg | 35 +++++++++--------- msg/SpawnPose.msg | 7 ---- msg/Spawnable.msg | 2 +- msg/SpawnableBounds.msg | 11 ------ package.xml | 2 +- srv/{GetSpawnPoses.srv => GetNamedPoses.srv} | 8 ++--- srv/GetSimulatorFeatures.srv | 2 +- srv/ResetSimulation.srv | 4 ++- srv/SetEntityState.srv | 1 + srv/SpawnEntity.srv | 37 +++++++++++++------- srv/StepSimulation.srv | 2 +- 19 files changed, 85 insertions(+), 65 deletions(-) create mode 100644 msg/NamedPose.msg create mode 100644 msg/PoseBounds.msg delete mode 100644 msg/SpawnPose.msg delete mode 100644 msg/SpawnableBounds.msg rename srv/{GetSpawnPoses.srv => GetNamedPoses.srv} (51%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 33b9902..a503a9d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,19 +19,19 @@ set(msg_files "msg/Entity.msg" "msg/EntityState.msg" "msg/EntityWithState.msg" + "msg/NamedPose.msg" + "msg/PoseBounds.msg" "msg/SimulatorFeatures.msg" "msg/Spawnable.msg" - "msg/SpawnableBounds.msg" - "msg/SpawnPose.msg" ) set(srv_files "srv/DeleteEntity.srv" "srv/GetEntities.srv" "srv/GetEntityState.srv" + "srv/GetNamedPoses.srv" "srv/GetSimulatorFeatures.srv" "srv/GetSpawnables.srv" - "srv/GetSpawnPoses.srv" "srv/ResetSimulation.srv" "srv/SetEntityState.srv" "srv/SetSimulationPaused.srv" diff --git a/README.md b/README.md index 69b3514..243cb19 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ # Simulation Interfaces Standard ROS 2 interfaces for interacting with simulators. -Messages and services are documented in their respective files. +Messages, services, and actions are documented in their respective files. diff --git a/action/MultiStepSimulation.action b/action/MultiStepSimulation.action index 0ddd69c..a3702e5 100644 --- a/action/MultiStepSimulation.action +++ b/action/MultiStepSimulation.action @@ -2,10 +2,10 @@ # 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). -uint16 steps # Action goal: step through the simulation loop this many times. +uint64 steps # Action goal: step through the simulation loop this many times. --- bool success # Return true if stepping is supported and was successful. string status_message # Additional comments or error status. # Calling with simulation not paused will return failure and an appropriate message. --- -uint16 completed_steps # Action feedback: number of completed steps in this action so far. +uint64 completed_steps # Action feedback: number of completed steps in this action so far. diff --git a/msg/Entity.msg b/msg/Entity.msg index f1718e8..86b61c2 100644 --- a/msg/Entity.msg +++ b/msg/Entity.msg @@ -1,4 +1,4 @@ -# Entity name and description +# Entity information string name # Scoped name of the entity. It must be unique (including namespace). string description # Optional: additional information about entity. diff --git a/msg/EntityState.msg b/msg/EntityState.msg index 7434cb0..a083753 100644 --- a/msg/EntityState.msg +++ b/msg/EntityState.msg @@ -1,4 +1,4 @@ -# Entity current pose and twist +# Entity current pose, twist and acceleration std_msgs/Header header # Frame and timestamp for pose and twist. Empty frame defaults to world. geometry_msgs/Pose pose # Pose in reference frame, ground truth. diff --git a/msg/EntityWithState.msg b/msg/EntityWithState.msg index 782482a..8431415 100644 --- a/msg/EntityWithState.msg +++ b/msg/EntityWithState.msg @@ -1,4 +1,5 @@ -# Entity with its current ground truth state +# Entity with its current ground truth state. Entities are objects in the simulation such as models and links. +# Each simulator might define what an entity is in a (slightly) different way. Entity entity # Entity name and description EntityState state # Entity current state diff --git a/msg/NamedPose.msg b/msg/NamedPose.msg new file mode 100644 index 0000000..0a0ac58 --- /dev/null +++ b/msg/NamedPose.msg @@ -0,0 +1,9 @@ +# 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. +PoseBounds bounds # Optional pose area bounds. diff --git a/msg/PoseBounds.msg b/msg/PoseBounds.msg new file mode 100644 index 0000000..e9f423b --- /dev/null +++ b/msg/PoseBounds.msg @@ -0,0 +1,11 @@ +# Bounds for named poses, e.g. to avoid spawning with other object overlap, or parking in a spot that is too small. +# Named pose might be valid for a small object, but not suitable for a large one, or a differently shaped one. +# These limits are relative to named pose or entity's top link transform, following ROS rep-103 conventions. + +# By default, both fields are zero vectors which means that pose is unbounded. +# Otherwise, the fields are expected to form a valid box containing the (0,0,0) point which is the position. +# 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. + +geometry_msgs/Vector3 upper_right # Optional: the upper right corner of the bounds box. +geometry_msgs/Vector3 lower_left # Optional: the lower left corner of the bounds box. diff --git a/msg/SimulatorFeatures.msg b/msg/SimulatorFeatures.msg index 0c1f8a0..a26e45d 100644 --- a/msg/SimulatorFeatures.msg +++ b/msg/SimulatorFeatures.msg @@ -2,32 +2,33 @@ uint8 SPAWNING = 0 # Supports spawn interface (SpawnEntity). uint8 DELETING = 1 # Supports deleting entities (DeleteEntity). -uint8 SPAWN_POSES = 2 # Supports predefined spawn poses (GetSpawnPoses). -uint8 SPAWN_BOUNDS = 3 # Supports spawn bounds (SpawnableBounds). +uint8 NAMED_POSES = 2 # Supports predefined named poses (GetNamedPoses). +uint8 POSE_BOUNDS = 3 # Supports pose bounds (PoseBounds). +uint8 SPAWNING_RESOURCE_STRING = 4 # Supports SpawnEntity resource_string field. -uint8 ENTITY_STATE_LISTING = 4 # Supports GetEntityState interface. -uint8 ENTITY_STATE_SETTING = 5 # Supports SetEntityState interface. +uint8 ENTITY_STATE_LISTING = 10 # Supports GetEntityState interface. +uint8 ENTITY_STATE_SETTING = 11 # Supports SetEntityState interface. -uint8 SIMULATION_RESET = 6 # Supports one or more ways to reset the simulation through ResetSimulation. -uint8 SIMULATION_RESET_TIME = 7 # Supports TIME flag for resetting. -uint8 SIMULATION_RESET_OBJECTS = 8 # Supports OBJECTS flag for resetting. -uint8 SIMULATION_RESET_SPAWNED = 9 # Supports SPAWNED flag for resetting. +uint8 SIMULATION_RESET = 20 # Supports one or more ways to reset the simulation through ResetSimulation. +uint8 SIMULATION_RESET_TIME = 21 # Supports TIME flag for resetting. +uint8 SIMULATION_RESET_STATE = 22 # Supports STATE flag for resetting. +uint8 SIMULATION_RESET_SPAWNED = 23 # Supports SPAWNED flag for resetting. -uint8 SIMULATION_PAUSE = 10 # Supports SetSimulationPaused interface -uint8 STEP_SIMULATION_SINGLE = 11 # Supports single stepping through simulation with StepSimulation interface. -uint8 STEP_SIMULATION_MULTIPLE = 12 # Supports multi-stepping through simulation, either through StepSimulation. +uint8 SIMULATION_PAUSE = 30 # Supports SetSimulationPaused 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 MultiStepSimulation action. -uint8 STEP_SIMULATION_ACTION = 13 # Supports MultiStepSimulation action interface. +uint8 STEP_SIMULATION_ACTION = 33 # Supports MultiStepSimulation action interface. -uint8[] features # A list of simulation features as specified by the list above. +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 +# 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: additional information for the caller, which could point to documentation, - # version compatibility and other useful meta information. +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/SpawnPose.msg b/msg/SpawnPose.msg deleted file mode 100644 index 86ac0e7..0000000 --- a/msg/SpawnPose.msg +++ /dev/null @@ -1,7 +0,0 @@ -# A named spawn pose - -string pose_name # Name of a spawn pose. It does not need to be unique. -string pose_description # Description for the user, e.g. "near the charging station". -geometry_msgs/Pose pose # Spawn pose relative to world, which can be used with SpawnEntity.srv. - -SpawnableBounds spawn_bounds # Optional spawn area bounds. diff --git a/msg/Spawnable.msg b/msg/Spawnable.msg index dbd92bc..4ff6eb3 100644 --- a/msg/Spawnable.msg +++ b/msg/Spawnable.msg @@ -2,4 +2,4 @@ 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". -SpawnableBounds spawn_bounds # Optional spawn area bounds which fully encompass this object. +PoseBounds spawn_bounds # Optional spawn area bounds which fully encompass this object. diff --git a/msg/SpawnableBounds.msg b/msg/SpawnableBounds.msg deleted file mode 100644 index dd516c4..0000000 --- a/msg/SpawnableBounds.msg +++ /dev/null @@ -1,11 +0,0 @@ -# Bounds for spawning an entity, e.g. to avoid spawning with other object overlap. -# Spawn pose might be valid for a small object, but not suitable for a large one, or a differently shaped one. -# These limits are relative to spawn pose or entity's top link transform, following ROS rep-103 conventions. - -# By default, both fields are zero vectors which means that spawning is unbounded. -# Otherwise, the fields are expected to form a valid box containing the (0,0,0) point which is the spawn position. -# You should check whether the entity simulation model fits within the bounds before spawning, to avoid overlaps and -# unstable behavior. - -geometry_msgs/Vector3 upper_right # Optional: the upper right corner of the spawn box. -geometry_msgs/Vector3 lower_left # Optional: the lower left corner of the spawn box. diff --git a/package.xml b/package.xml index a8a6589..a3b3729 100644 --- a/package.xml +++ b/package.xml @@ -3,7 +3,7 @@ simulation_interfaces 1.0.0 - A package containing simulation interfaces including messages and services + A package containing simulation interfaces including messages, services and actions Adam Dabrowski Apache License 2.0 https://github.com/ros-simulation/simulation-interfaces diff --git a/srv/GetSpawnPoses.srv b/srv/GetNamedPoses.srv similarity index 51% rename from srv/GetSpawnPoses.srv rename to srv/GetNamedPoses.srv index b7d03cf..d7a004b 100644 --- a/srv/GetSpawnPoses.srv +++ b/srv/GetNamedPoses.srv @@ -1,10 +1,10 @@ -# Get predefined spawn poses which is convenient to avoid spawning in invalid spaces. -# This is an optional simulation feature: check status_message if you receive an empty spawn_poses vector, -# to determine whether this feature is not supported by your simulator, or spawn poses are simply not defined. +# Get predefined poses which are convenient to for spawning, navigation goals etc. +# This is an optional simulation feature: check status_message if you receive an empty poses vector, +# to determine whether this feature is not supported by your simulator, or named poses are simply not defined. --- -SpawnPose[] spawn_poses # A list of predefined spawn_poses, which may be empty. +NamedPose[] poses # A list of predefined poses, which may be empty. bool success # Return true if successful. string status_message # Additional comments or error status. diff --git a/srv/GetSimulatorFeatures.srv b/srv/GetSimulatorFeatures.srv index cce072a..785efa8 100644 --- a/srv/GetSimulatorFeatures.srv +++ b/srv/GetSimulatorFeatures.srv @@ -3,4 +3,4 @@ --- -SimulatorFeatures simulator_features +SimulatorFeatures features diff --git a/srv/ResetSimulation.srv b/srv/ResetSimulation.srv index c83a92d..143d148 100644 --- a/srv/ResetSimulation.srv +++ b/srv/ResetSimulation.srv @@ -3,11 +3,13 @@ uint8 DEFAULT=0 # same as ALL. uint8 TIME=1 # Reset simulation time to start. -uint8 OBJECTS=2 # Reset all poses and velocities. +uint8 STATE=2 # Reset state such as poses and velocities. This may include state randomization + # if such feature is available and turned on. uint8 SPAWNED=4 # De-spawns all spawned entities. uint8 ALL=7 # Fully resets simulation to the start, as if it was closed and launched again. uint8 reset_scope # Scope of the reset. Note that simulators might only support some types of resets. + # This is a bit field which you can check for each scope e.g. reset_scope & TIME. --- diff --git a/srv/SetEntityState.srv b/srv/SetEntityState.srv index 30907c6..b54440d 100644 --- a/srv/SetEntityState.srv +++ b/srv/SetEntityState.srv @@ -2,6 +2,7 @@ string name # Unique name as returned by GetEntities or SpawnEntity. EntityState state # New state to set immediately. The timestamp in header is ignored. + # Note that the acceleration field may be ignored by simulators. --- diff --git a/srv/SpawnEntity.srv b/srv/SpawnEntity.srv index b920483..3893239 100644 --- a/srv/SpawnEntity.srv +++ b/srv/SpawnEntity.srv @@ -1,19 +1,32 @@ # Spawn an entity (a robot, other object) by name or URI -string name # Suggest a name to give to the spawned entity. If left empty, it will be generated. - # Due to requirement for name uniqueness and the namespace, check the status_message - # to get an actual unique name to use for further queries. -string uri # Resource such as SDF or URDF file, or in native format such as prefab. - # Valid URIs can be determined by calling GetSpawnables first, - # if the feature is supported by your simulator. -string entity_namespace # Spawn the entity with all its interfaces under this namespace. -string reference_frame # Optional initial pose reference frame. Defaults to global "world" frame. - # This ROS frame must be known to the simulator, e.g. of an object spawned earlier. -geometry_msgs/Pose initial_pose # Initial entity pose. Simulators should spawn the entity with its top-level link - # transform matching this pose. +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 in the status_message. +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). + # Check whether it is supported by your simulator through GetSimulatorFeatures. +string entity_namespace # Spawn the entity with all its interfaces under this namespace. +geometry_msgs/PoseStamped initial_pose # Initial entity pose. Simulators will spawn the entity with its top-level link + # transform matching this 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. --- bool success # Return true if spawned successfully. string status_message # On failure, a user-friendly error message. -string unique_name # Spawned entity full name (with namespace). +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 index 5919e21..6e3a05f 100644 --- a/srv/StepSimulation.srv +++ b/srv/StepSimulation.srv @@ -2,7 +2,7 @@ # The service only returns once stepping is complete, which might take considerable time. # There is an alternative in MultiStepSimulation action, which is worth considering for a multi-step interface. -uint16 steps # Step through the simulation loop this many times. +uint64 steps # Step through the simulation loop this many times. --- From e94797f96648c2560d2de22dbdcbd2148c4216ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Fri, 24 Jan 2025 17:04:50 +0100 Subject: [PATCH 17/33] Added sources field; documentation changes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- srv/GetSpawnables.srv | 8 ++++++++ srv/SpawnEntity.srv | 4 ++-- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/srv/GetSpawnables.srv b/srv/GetSpawnables.srv index 882b4be..bb7d49d 100644 --- a/srv/GetSpawnables.srv +++ b/srv/GetSpawnables.srv @@ -4,6 +4,14 @@ --- Spawnable[] spawnables # Spawnable objects with URI and additional information. +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 status_message, + # but do not hinder success of the response. + # Sources may include subcategories and be simulator-specific. bool success # Return true if successful. string status_message # Additional comments or error status. diff --git a/srv/SpawnEntity.srv b/srv/SpawnEntity.srv index 3893239..1bea211 100644 --- a/srv/SpawnEntity.srv +++ b/srv/SpawnEntity.srv @@ -17,9 +17,9 @@ string uri # Resource such as SDFormat, URDF, USD o 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). # Check whether it is supported by your simulator through GetSimulatorFeatures. + # 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. Simulators will spawn the entity with its top-level link - # transform matching this pose. +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. From eabed903ffb31be55dfc1a0aedc9cb41ec91b940 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Wed, 5 Feb 2025 16:08:01 +0100 Subject: [PATCH 18/33] Update action/MultiStepSimulation.action Co-authored-by: Tully Foote --- action/MultiStepSimulation.action | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/action/MultiStepSimulation.action b/action/MultiStepSimulation.action index a3702e5..ca56e30 100644 --- a/action/MultiStepSimulation.action +++ b/action/MultiStepSimulation.action @@ -8,4 +8,5 @@ bool success # Return true if stepping is supported and w string status_message # Additional comments or error status. # Calling with simulation not paused will return failure and an appropriate message. --- -uint64 completed_steps # Action feedback: number of completed steps in this action so far. +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. From 271a8a10e41265a45f6094c178bfd336f68a4f04 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Wed, 5 Feb 2025 16:08:24 +0100 Subject: [PATCH 19/33] Modified result handling and entity info, tags and categories - Result is now code-based and extendable. - Generic result message is defined with uint8 and string parts - Result codes extension is in place, with specific codes for services. - Entity message is removed as it is handled as string everywhere. - EntityInfo and GetEntityInfo is added, with tags and categories. - Filtering for tags and categories is added. - Review suggestions applied. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- CMakeLists.txt | 3 ++- action/MultiStepSimulation.action | 6 +++--- msg/Entity.msg | 4 ---- msg/EntityInfo.msg | 14 ++++++++++++++ msg/EntityWithState.msg | 4 ++-- msg/NamedPose.msg | 2 +- msg/Result.msg | 18 ++++++++++++++++++ srv/DeleteEntity.srv | 3 +-- srv/GetEntities.srv | 19 ++++++++++++++++--- srv/GetEntityInfo.srv | 8 ++++++++ srv/GetEntityState.srv | 6 ++---- srv/GetNamedPoses.srv | 4 +--- srv/GetSpawnables.srv | 13 ++++++------- srv/ResetSimulation.srv | 13 ++++++------- srv/SetEntityState.srv | 3 +-- srv/SetSimulationPaused.srv | 8 +++++--- srv/SpawnEntity.srv | 24 +++++++++++++++++++----- srv/StepSimulation.srv | 4 +--- 18 files changed, 106 insertions(+), 50 deletions(-) delete mode 100644 msg/Entity.msg create mode 100644 msg/EntityInfo.msg create mode 100644 msg/Result.msg create mode 100644 srv/GetEntityInfo.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index a503a9d..712aa64 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,11 +16,12 @@ find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) set(msg_files - "msg/Entity.msg" + "msg/EntityInfo.msg" "msg/EntityState.msg" "msg/EntityWithState.msg" "msg/NamedPose.msg" "msg/PoseBounds.msg" + "msg/Result.msg" "msg/SimulatorFeatures.msg" "msg/Spawnable.msg" ) diff --git a/action/MultiStepSimulation.action b/action/MultiStepSimulation.action index ca56e30..29588a9 100644 --- a/action/MultiStepSimulation.action +++ b/action/MultiStepSimulation.action @@ -4,9 +4,9 @@ uint64 steps # Action goal: step through the simulation loop this many times. --- -bool success # Return true if stepping is supported and was successful. -string status_message # Additional comments or error status. - # Calling with simulation not paused will return failure and an appropriate message. + +Result result # Calling with simulation not paused 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/Entity.msg b/msg/Entity.msg deleted file mode 100644 index 86b61c2..0000000 --- a/msg/Entity.msg +++ /dev/null @@ -1,4 +0,0 @@ -# Entity information - -string name # Scoped name of the entity. It must be unique (including namespace). -string description # Optional: additional information about entity. diff --git a/msg/EntityInfo.msg b/msg/EntityInfo.msg new file mode 100644 index 0000000..6c6c0d6 --- /dev/null +++ b/msg/EntityInfo.msg @@ -0,0 +1,14 @@ +# Entity type and additional information + +uint8 OBJECT = 0 # Generic or unspecified type. +uint8 ROBOT = 1 # A broad category for mobile robots, arms, drones etc., usually with ROS 2 interfaces. +uint8 HUMAN = 2 # Simulated humans, e.g. pedestrians, warehouse workers. Compared to DYNAMIC_OBJECT category, + # humans are often expected to be treated exceptionally in regards to safety constraints. +uint8 DYNAMIC_OBJECT = 4 # Vehicles, animals, mobile obstacles, typically to present a detection & tracking challenge, + # not themselves subject to validation. +uint8 STATIC_OBJECT = 5 # Any object which is static, e.g. not supposed to change its pose + # unless by means of SetEntityState. + +uint8 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. Can be used for LLMs. +string[] tags # optional: tags which are useful for filtering and categorizing entities further. diff --git a/msg/EntityWithState.msg b/msg/EntityWithState.msg index 8431415..8644888 100644 --- a/msg/EntityWithState.msg +++ b/msg/EntityWithState.msg @@ -1,5 +1,5 @@ # Entity with its current ground truth state. Entities are objects in the simulation such as models and links. # Each simulator might define what an entity is in a (slightly) different way. -Entity entity # Entity name and description -EntityState state # Entity current state +string name # Entity unique name. +EntityState state # Entity current state. diff --git a/msg/NamedPose.msg b/msg/NamedPose.msg index 0a0ac58..855d832 100644 --- a/msg/NamedPose.msg +++ b/msg/NamedPose.msg @@ -6,4 +6,4 @@ string[] tags # Optional tags which can be used to d # 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. -PoseBounds bounds # Optional pose area bounds. +PoseBounds bounds # Pose area bounds, which are empty by default, meaning pose is unbounded. diff --git a/msg/Result.msg b/msg/Result.msg new file mode 100644 index 0000000..9f7e103 --- /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 FEATURE_UNSUPPORTED = 0 # Feature is not supported by the simulator, check GetSimulatorFeatures. + # While feature support can sometimes be deducted by last of corresponding + # service / action interface, in other cases it is about supporting certain call + # parameters, formats and options within interface call. +uint8 OK = 1 +uint8 NOT_FOUND = 2 # No match for input (such as when entity name does not exist). +uint8 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 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/srv/DeleteEntity.srv b/srv/DeleteEntity.srv index a8f1200..b837a03 100644 --- a/srv/DeleteEntity.srv +++ b/srv/DeleteEntity.srv @@ -4,5 +4,4 @@ string name # Unique name with a namespace, as returned --- -bool success # return true if deleted successfully. -string status_message # On failure, a user-friendly error message. +Result result diff --git a/srv/GetEntities.srv b/srv/GetEntities.srv index 2a7e0a7..db407dc 100644 --- a/srv/GetEntities.srv +++ b/srv/GetEntities.srv @@ -3,10 +3,23 @@ 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. + # Applies together with other filters (categories, tags). +uint8[] 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). +string[] tags # Optional, defaults to empty, which means no tags filter. + # Entities matching any or all of tags will be returned, depending + # on tags_filter_mode. To get entity tags, use GetEntityInfo. + # Applies together with other filters (filter, categories). + +uint8 ANY_OF = 0 # Filter with AND mode (all tags need to match). +uint8 ALL_OF = 1 # Filter with OR mode (any tag can match). + +uint8 tags_filter_mode # Set to control filter application for tags. + --- +Result result EntityWithState[] entities # All entities with their states, matching the filter. - -bool success # Return true if successful. -string status_message # Additional comments or error status. diff --git a/srv/GetEntityInfo.srv b/srv/GetEntityInfo.srv new file mode 100644 index 0000000..33c53e1 --- /dev/null +++ b/srv/GetEntityInfo.srv @@ -0,0 +1,8 @@ +# Get type and other information about an entity. + +string name # Unique name as returned by GetEntities / SpawnEntity. + +--- + +Result result +EntityInfo info # Only valid if result.result_code is OK. diff --git a/srv/GetEntityState.srv b/srv/GetEntityState.srv index 17faae8..16bc257 100644 --- a/srv/GetEntityState.srv +++ b/srv/GetEntityState.srv @@ -4,7 +4,5 @@ string name # Unique name as returne --- -EntityState entity_state # Entity ground truth state. - -bool success # Return true if successful. -string status_message # Additional comments or error status. +Result result +EntityState state # Entity ground truth state. Valid when result.result = OK. diff --git a/srv/GetNamedPoses.srv b/srv/GetNamedPoses.srv index d7a004b..a0e49a8 100644 --- a/srv/GetNamedPoses.srv +++ b/srv/GetNamedPoses.srv @@ -4,7 +4,5 @@ --- +Result result NamedPose[] poses # A list of predefined poses, which may be empty. - -bool success # Return true if successful. -string status_message # Additional comments or error status. diff --git a/srv/GetSpawnables.srv b/srv/GetSpawnables.srv index bb7d49d..0009847 100644 --- a/srv/GetSpawnables.srv +++ b/srv/GetSpawnables.srv @@ -1,17 +1,16 @@ # 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 status_message. +# This interface is an optional extension and might not be implemented by your simulator, check the result_code. ---- - -Spawnable[] spawnables # Spawnable objects with URI and additional information. 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 status_message, + # 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. -bool success # Return true if successful. -string status_message # Additional comments or error status. +--- + +Result result +Spawnable[] spawnables # Spawnable objects with URI and additional information. diff --git a/srv/ResetSimulation.srv b/srv/ResetSimulation.srv index 143d148..c967ad4 100644 --- a/srv/ResetSimulation.srv +++ b/srv/ResetSimulation.srv @@ -1,17 +1,16 @@ # Reset the simulation to the start, including the entire scene and the simulation time. # Objects that were dynamically spawned are de-spawned. -uint8 DEFAULT=0 # same as ALL. -uint8 TIME=1 # Reset simulation time to start. -uint8 STATE=2 # Reset state such as poses and velocities. This may include state randomization +uint8 DEFAULT = 0 # same as ALL. +uint8 TIME = 1 # Reset simulation time to start. +uint8 STATE = 2 # Reset state such as poses and velocities. This may include state randomization # if such feature is available and turned on. -uint8 SPAWNED=4 # De-spawns all spawned entities. -uint8 ALL=7 # Fully resets simulation to the start, as if it was closed and launched again. +uint8 SPAWNED = 4 # De-spawns all spawned entities. +uint8 ALL = 7 # Fully resets simulation to the start, as if it was closed and launched again. uint8 reset_scope # Scope of the reset. Note that simulators might only support some types of resets. # This is a bit field which you can check for each scope e.g. reset_scope & TIME. --- -bool success # Return true if successful. -string status_message # Additional comments or error status, such as what reset types are supported. +Result result diff --git a/srv/SetEntityState.srv b/srv/SetEntityState.srv index b54440d..fe600cd 100644 --- a/srv/SetEntityState.srv +++ b/srv/SetEntityState.srv @@ -6,5 +6,4 @@ EntityState state # New state to set immediately. The time --- -bool success # Return true if successful. -string status_message # Additional comments or error status. +Result result diff --git a/srv/SetSimulationPaused.srv b/srv/SetSimulationPaused.srv index 52aea70..074cb00 100644 --- a/srv/SetSimulationPaused.srv +++ b/srv/SetSimulationPaused.srv @@ -1,9 +1,11 @@ # Pauses or unpauses the simulation bool pause # If true, sets simulation state to paused, otherwise sets it to running. - # If already in target state, nothing happens but status_message informs about it. + # If already in target state, nothing happens but the result informs about it. --- -bool success # Return true if pausing / resuming was successful. -string status_message # Additional comments or error status. +uint8 ALREADY_PAUSED = 101 # Additional result type for this call, which means simulation was already + # paused and remains so. + +Result result \ No newline at end of file diff --git a/srv/SpawnEntity.srv b/srv/SpawnEntity.srv index 1bea211..9dbfe67 100644 --- a/srv/SpawnEntity.srv +++ b/srv/SpawnEntity.srv @@ -6,7 +6,7 @@ string name # A name to give to the spawned entity. # 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 in the status_message. + # 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. @@ -26,7 +26,21 @@ geometry_msgs/PoseStamped initial_pose # Initial entity pose. --- -bool success # Return true if spawned successfully. -string status_message # On failure, a user-friendly error message. -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. +# 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 index 6e3a05f..423f7a6 100644 --- a/srv/StepSimulation.srv +++ b/srv/StepSimulation.srv @@ -6,6 +6,4 @@ uint64 steps # Step through the simulation loop this many --- -bool success # Return true if stepping is supported and was successful. -string status_message # Additional comments or error status. - # Calling with simulation not paused will return failure and an appropriate message. +Result result # Calling with simulation not paused will return OPERATION_FAILED and error message. From 9ec77e64649649a4890e67986dc74c34a23d0fac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Thu, 6 Feb 2025 10:16:11 +0100 Subject: [PATCH 20/33] Apply suggestions from code review Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Co-authored-by: Martin Pecka Co-authored-by: David V. Lu!! --- action/MultiStepSimulation.action | 2 +- msg/EntityInfo.msg | 2 +- msg/Result.msg | 2 +- srv/ResetSimulation.srv | 2 +- srv/SetSimulationPaused.srv | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/action/MultiStepSimulation.action b/action/MultiStepSimulation.action index 29588a9..572a5bb 100644 --- a/action/MultiStepSimulation.action +++ b/action/MultiStepSimulation.action @@ -5,7 +5,7 @@ uint64 steps # Action goal: step through the simulation loop this many times. --- -Result result # Calling with simulation not paused will return OPERATION_FAILED and error message. +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. diff --git a/msg/EntityInfo.msg b/msg/EntityInfo.msg index 6c6c0d6..2be50e1 100644 --- a/msg/EntityInfo.msg +++ b/msg/EntityInfo.msg @@ -10,5 +10,5 @@ uint8 STATIC_OBJECT = 5 # Any object which is static, e.g. not supposed to c # unless by means of SetEntityState. uint8 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. Can be used for LLMs. +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/Result.msg b/msg/Result.msg index 9f7e103..7483551 100644 --- a/msg/Result.msg +++ b/msg/Result.msg @@ -2,7 +2,7 @@ # Note that additional results for specific services can defined within them using values above 100. uint8 FEATURE_UNSUPPORTED = 0 # Feature is not supported by the simulator, check GetSimulatorFeatures. - # While feature support can sometimes be deducted by last of corresponding + # 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 OK = 1 diff --git a/srv/ResetSimulation.srv b/srv/ResetSimulation.srv index c967ad4..1217250 100644 --- a/srv/ResetSimulation.srv +++ b/srv/ResetSimulation.srv @@ -6,7 +6,7 @@ uint8 TIME = 1 # Reset simulation time to start. uint8 STATE = 2 # Reset state such as poses and velocities. This may include state randomization # if such feature is available and turned on. uint8 SPAWNED = 4 # De-spawns all spawned entities. -uint8 ALL = 7 # Fully resets simulation to the start, as if it was closed and launched again. +uint8 ALL = 255 # Fully resets simulation to the start, as if it was closed and launched again. uint8 reset_scope # Scope of the reset. Note that simulators might only support some types of resets. # This is a bit field which you can check for each scope e.g. reset_scope & TIME. diff --git a/srv/SetSimulationPaused.srv b/srv/SetSimulationPaused.srv index 074cb00..bb8af48 100644 --- a/srv/SetSimulationPaused.srv +++ b/srv/SetSimulationPaused.srv @@ -8,4 +8,4 @@ bool pause # If true, sets simulation state to paused, uint8 ALREADY_PAUSED = 101 # Additional result type for this call, which means simulation was already # paused and remains so. -Result result \ No newline at end of file +Result result From 86ea8e04e9121bb43af5916bc5f5c9c0a6829c0b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Thu, 6 Feb 2025 12:41:53 +0100 Subject: [PATCH 21/33] Addressing review: - Updated min cmake version - Created EntityCategories to capture constants. - Extracted TagsFilter into a message, reused in GetNamedPoses. - Other minor changes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- CMakeLists.txt | 4 +++- msg/EntityCategories.msg | 11 +++++++++++ msg/EntityInfo.msg | 10 +--------- msg/TagsFilter.msg | 10 ++++++++++ srv/GetEntities.srv | 12 ++---------- srv/GetNamedPoses.srv | 3 +++ srv/ResetSimulation.srv | 4 ++-- 7 files changed, 32 insertions(+), 22 deletions(-) create mode 100644 msg/EntityCategories.msg create mode 100644 msg/TagsFilter.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 712aa64..1aa92a2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.16) project(simulation_interfaces) if(NOT CMAKE_CXX_STANDARD) @@ -16,6 +16,7 @@ find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) set(msg_files + "msg/EntityCategories.msg" "msg/EntityInfo.msg" "msg/EntityState.msg" "msg/EntityWithState.msg" @@ -24,6 +25,7 @@ set(msg_files "msg/Result.msg" "msg/SimulatorFeatures.msg" "msg/Spawnable.msg" + "msg/TagsFilter.msg" ) set(srv_files diff --git a/msg/EntityCategories.msg b/msg/EntityCategories.msg new file mode 100644 index 0000000..b1e29bb --- /dev/null +++ b/msg/EntityCategories.msg @@ -0,0 +1,11 @@ +# Constants for entity categories + +uint8 OBJECT = 0 # Generic or unspecified type. +uint8 ROBOT = 1 # A broad category for mobile robots, arms, drones etc., usually with ROS 2 interfaces. +uint8 HUMAN = 2 # Simulated humans, e.g. pedestrians, warehouse workers. Compared to DYNAMIC_OBJECT category, + # humans are often expected to be treated exceptionally in regards to safety constraints. +uint8 DYNAMIC_OBJECT = 4 # Vehicles, animals, mobile obstacles, typically to present a detection & tracking challenge, + # not themselves subject to validation. +uint8 STATIC_OBJECT = 5 # Any object which is static, e.g. not supposed to change its pose + # unless by means of SetEntityState. + diff --git a/msg/EntityInfo.msg b/msg/EntityInfo.msg index 2be50e1..4a4c526 100644 --- a/msg/EntityInfo.msg +++ b/msg/EntityInfo.msg @@ -1,14 +1,6 @@ # Entity type and additional information -uint8 OBJECT = 0 # Generic or unspecified type. -uint8 ROBOT = 1 # A broad category for mobile robots, arms, drones etc., usually with ROS 2 interfaces. -uint8 HUMAN = 2 # Simulated humans, e.g. pedestrians, warehouse workers. Compared to DYNAMIC_OBJECT category, - # humans are often expected to be treated exceptionally in regards to safety constraints. -uint8 DYNAMIC_OBJECT = 4 # Vehicles, animals, mobile obstacles, typically to present a detection & tracking challenge, - # not themselves subject to validation. -uint8 STATIC_OBJECT = 5 # Any object which is static, e.g. not supposed to change its pose - # unless by means of SetEntityState. - uint8 category # Major category for the entity. Extra entity type distinction can be made through tags. + # See EntityCategories for defined category values. 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/TagsFilter.msg b/msg/TagsFilter.msg new file mode 100644 index 0000000..4a6a3d9 --- /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 ANY_OF = 0 # Filter with AND mode (all tags need to match). +uint8 ALL_OF = 1 # Filter with OR mode (any tag can match). + +uint8 tags_filter_mode # Set to control filter application for tags. diff --git a/srv/GetEntities.srv b/srv/GetEntities.srv index db407dc..46b7f21 100644 --- a/srv/GetEntities.srv +++ b/srv/GetEntities.srv @@ -8,18 +8,10 @@ uint8[] categories # Optional, defaults to empt # Entities matching any of the categories will be returned. # To get entity category, use GetEntityInfo. # Applies together with other filters (filter, tags). -string[] tags # Optional, defaults to empty, which means no tags filter. - # Entities matching any or all of tags will be returned, depending - # on tags_filter_mode. To get entity tags, use GetEntityInfo. +TagsFilter tags # Tags filter to apply. To get entity tags, use GetEntityInfo. # Applies together with other filters (filter, categories). -uint8 ANY_OF = 0 # Filter with AND mode (all tags need to match). -uint8 ALL_OF = 1 # Filter with OR mode (any tag can match). - -uint8 tags_filter_mode # Set to control filter application for tags. - - --- Result result -EntityWithState[] entities # All entities with their states, matching the filter. +EntityWithState[] entities # All entities with their states, matching the filters. diff --git a/srv/GetNamedPoses.srv b/srv/GetNamedPoses.srv index a0e49a8..f026eaa 100644 --- a/srv/GetNamedPoses.srv +++ b/srv/GetNamedPoses.srv @@ -2,6 +2,9 @@ # This is an optional simulation feature: check status_message if you receive an empty poses vector, # to determine whether this feature is not supported by your simulator, or named poses are simply not defined. +TagsFilter tags # Tags filter to apply. Only named poses with matching tags field + # will be returned. Can be empty (see TagsFilter). + --- Result result diff --git a/srv/ResetSimulation.srv b/srv/ResetSimulation.srv index 1217250..8c16b39 100644 --- a/srv/ResetSimulation.srv +++ b/srv/ResetSimulation.srv @@ -6,10 +6,10 @@ uint8 TIME = 1 # Reset simulation time to start. uint8 STATE = 2 # Reset state such as poses and velocities. This may include state randomization # if such feature is available and turned on. uint8 SPAWNED = 4 # De-spawns all spawned entities. -uint8 ALL = 255 # Fully resets simulation to the start, as if it was closed and launched again. +uint8 ALL = 255 # Fully resets simulation to the start, as if it was closed and launched again. uint8 reset_scope # Scope of the reset. Note that simulators might only support some types of resets. - # This is a bit field which you can check for each scope e.g. reset_scope & TIME. + # This is a bit field which may be checked for each scope e.g. reset_scope & TIME. --- From 9d123ed6b5a74fade1af872a72e4b6da69d77481 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Fri, 7 Feb 2025 16:06:20 +0100 Subject: [PATCH 22/33] Addressing review comments: - Added Entity message, EntityWithState changed to just EntityState - Separated out EntityFilters - There are now both GetEntities and GetEntitiesStates services. - NamedPose bounds are now not included in GetNamedPoses, but instead there is a GetNamedPoseBounds service. - PoseBounds are now Bounds, and are used for Spawnables, Poses and Entities, and are more generic. - New interface GetEntityBounds along with feature type. - EntityFilters now include an overlap filter, a practical one. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- CMakeLists.txt | 9 +++++++-- msg/Bounds.msg | 24 ++++++++++++++++++++++++ msg/Entity.msg | 4 ++++ msg/EntityFilters.msg | 17 +++++++++++++++++ msg/EntityWithState.msg | 5 ----- msg/NamedPose.msg | 1 - msg/PoseBounds.msg | 11 ----------- msg/SimulatorFeatures.msg | 5 +++-- msg/Spawnable.msg | 2 +- srv/GetEntities.srv | 18 ++++++------------ srv/GetEntitiesStates.srv | 11 +++++++++++ srv/GetEntityBounds.srv | 8 ++++++++ srv/GetNamedPoseBounds.srv | 8 ++++++++ 13 files changed, 89 insertions(+), 34 deletions(-) create mode 100644 msg/Bounds.msg create mode 100644 msg/Entity.msg create mode 100644 msg/EntityFilters.msg delete mode 100644 msg/EntityWithState.msg delete mode 100644 msg/PoseBounds.msg create mode 100644 srv/GetEntitiesStates.srv create mode 100644 srv/GetEntityBounds.srv create mode 100644 srv/GetNamedPoseBounds.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 1aa92a2..2558c33 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,12 +16,13 @@ find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) set(msg_files + "msg/Bounds.msg" + "msg/Entity.msg" "msg/EntityCategories.msg" + "msg/EntityFilters.msg" "msg/EntityInfo.msg" "msg/EntityState.msg" - "msg/EntityWithState.msg" "msg/NamedPose.msg" - "msg/PoseBounds.msg" "msg/Result.msg" "msg/SimulatorFeatures.msg" "msg/Spawnable.msg" @@ -31,7 +32,11 @@ set(msg_files 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/GetSimulatorFeatures.srv" "srv/GetSpawnables.srv" diff --git a/msg/Bounds.msg b/msg/Bounds.msg new file mode 100644 index 0000000..eda04ba --- /dev/null +++ b/msg/Bounds.msg @@ -0,0 +1,24 @@ +# 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 top link transform, following ROS rep-103 convention. + +# Empty bounds are to be understood as "unbounded". +# Otherwise, the fields are expected to define a valid area containing the (0,0,0) point which is the position. +# 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 BOX = 0 # Bounding box, points field should have two values, which are + # upper right and lower left corners of the box. +uint8 CONVEX_HULL = 1 # Points define a convex hull (at least 3 non-collinear points). +uint8 SPHERE = 2 # 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 values are ignored). + + +uint8 type # See type value documentation. +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/Entity.msg b/msg/Entity.msg new file mode 100644 index 0000000..10faee9 --- /dev/null +++ b/msg/Entity.msg @@ -0,0 +1,4 @@ +# Entity identified by its unique name. Entities are objects in the simulation such as models and links. +# Each simulator might define what an entity is in a (slightly) different way. + +string name # Entity unique name. diff --git a/msg/EntityFilters.msg b/msg/EntityFilters.msg new file mode 100644 index 0000000..53d0f5a --- /dev/null +++ b/msg/EntityFilters.msg @@ -0,0 +1,17 @@ +# 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. + # Applies together with other filters (categories, tags). +uint8[] 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). +TagsFilter tags # Tags filter to apply. To get entity tags, use GetEntityInfo. + # Applies together with other filters (filter, categories). + +# if overlap_radius > 0, the overlap filter is applied and entities overlapping with the defined sphere will be returned. + +float64 overlap_radius # Radius of the overlap sphere +geometry_msgs/Point overlap_center # Center point of the overlap sphere, in world coordinates. diff --git a/msg/EntityWithState.msg b/msg/EntityWithState.msg deleted file mode 100644 index 8644888..0000000 --- a/msg/EntityWithState.msg +++ /dev/null @@ -1,5 +0,0 @@ -# Entity with its current ground truth state. Entities are objects in the simulation such as models and links. -# Each simulator might define what an entity is in a (slightly) different way. - -string name # Entity unique name. -EntityState state # Entity current state. diff --git a/msg/NamedPose.msg b/msg/NamedPose.msg index 855d832..54dec31 100644 --- a/msg/NamedPose.msg +++ b/msg/NamedPose.msg @@ -6,4 +6,3 @@ string[] tags # Optional tags which can be used to d # 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. -PoseBounds bounds # Pose area bounds, which are empty by default, meaning pose is unbounded. diff --git a/msg/PoseBounds.msg b/msg/PoseBounds.msg deleted file mode 100644 index e9f423b..0000000 --- a/msg/PoseBounds.msg +++ /dev/null @@ -1,11 +0,0 @@ -# Bounds for named poses, e.g. to avoid spawning with other object overlap, or parking in a spot that is too small. -# Named pose might be valid for a small object, but not suitable for a large one, or a differently shaped one. -# These limits are relative to named pose or entity's top link transform, following ROS rep-103 conventions. - -# By default, both fields are zero vectors which means that pose is unbounded. -# Otherwise, the fields are expected to form a valid box containing the (0,0,0) point which is the position. -# 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. - -geometry_msgs/Vector3 upper_right # Optional: the upper right corner of the bounds box. -geometry_msgs/Vector3 lower_left # Optional: the lower left corner of the bounds box. diff --git a/msg/SimulatorFeatures.msg b/msg/SimulatorFeatures.msg index a26e45d..4a62c09 100644 --- a/msg/SimulatorFeatures.msg +++ b/msg/SimulatorFeatures.msg @@ -3,8 +3,9 @@ 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 (PoseBounds). -uint8 SPAWNING_RESOURCE_STRING = 4 # Supports SpawnEntity resource_string field. +uint8 POSE_BOUNDS = 3 # Supports pose bounds (GetNamedPoseBounds). +uint8 ENTITY_BOUNDS = 4 # Supports entity bounds (GetEntityBounds). +uint8 SPAWNING_RESOURCE_STRING = 5 # Supports SpawnEntity resource_string field. uint8 ENTITY_STATE_LISTING = 10 # Supports GetEntityState interface. uint8 ENTITY_STATE_SETTING = 11 # Supports SetEntityState interface. diff --git a/msg/Spawnable.msg b/msg/Spawnable.msg index 4ff6eb3..c89eae2 100644 --- a/msg/Spawnable.msg +++ b/msg/Spawnable.msg @@ -2,4 +2,4 @@ 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". -PoseBounds spawn_bounds # Optional spawn area bounds which fully encompass this object. +Bounds spawn_bounds # Optional spawn area bounds which fully encompass this object. diff --git a/srv/GetEntities.srv b/srv/GetEntities.srv index 46b7f21..dc96012 100644 --- a/srv/GetEntities.srv +++ b/srv/GetEntities.srv @@ -1,17 +1,11 @@ -# Get objects in the scene which can be interacted, e.g. with using SetEntityState. +# 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. -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. - # Applies together with other filters (categories, tags). -uint8[] 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). -TagsFilter tags # Tags filter to apply. To get entity tags, use GetEntityInfo. - # Applies together with other filters (filter, categories). +EntityFilters filters # Optional filters for the query, including name, category, tags, + # and overlap filters. --- Result result -EntityWithState[] entities # All entities with their states, matching the filters. +Entity[] entities # All entities matching the filters. diff --git a/srv/GetEntitiesStates.srv b/srv/GetEntitiesStates.srv new file mode 100644 index 0000000..dddf860 --- /dev/null +++ b/srv/GetEntitiesStates.srv @@ -0,0 +1,11 @@ +# 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. + +EntityFilters filters # Optional filters for the query, including name, category, tags, + # and overlap filters. + +--- + +Result result +Entity[] entities # 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..0ddc60e --- /dev/null +++ b/srv/GetEntityBounds.srv @@ -0,0 +1,8 @@ +# Get geometrical bounds for entity. This feature is available if GetSimulatorFeatures includes ENTITY_BOUNDS feature. + +string name # Unique name as returned by GetEntities / SpawnEntity. + +--- + +Result result +Bounds bounds # Entity bounds. Only valid if result.result_code is OK. diff --git a/srv/GetNamedPoseBounds.srv b/srv/GetNamedPoseBounds.srv new file mode 100644 index 0000000..a30d515 --- /dev/null +++ b/srv/GetNamedPoseBounds.srv @@ -0,0 +1,8 @@ +# Get bounds for the named pose. This feature is available if GetSimulatorFeatures includes POSE_BOUNDS feature. + +string name # unique names (as returned from GetNamedPoses). + +--- + +Result result +Bounds bounds # bounds for the named pose. From a940c65e1422819f03f6a36c713f627868cbf34e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Mon, 10 Feb 2025 15:30:46 +0100 Subject: [PATCH 23/33] Review comments applied: - Changed from string to Entity type for entity interfaces. - Added SimulationState SetSimulationState and GetSimulationState interfaces. - Clarified twist reference frame as suggested. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- CMakeLists.txt | 4 +++- msg/EntityState.msg | 4 ++-- msg/SimulationState.msg | 16 ++++++++++++++++ srv/DeleteEntity.srv | 5 +++-- srv/GetEntityBounds.srv | 2 +- srv/GetEntityInfo.srv | 2 +- srv/GetEntityState.srv | 4 ++-- srv/GetSimulationState.srv | 7 +++++++ srv/SetEntityState.srv | 2 +- srv/SetSimulationPaused.srv | 11 ----------- srv/SetSimulationState.srv | 15 +++++++++++++++ 11 files changed, 51 insertions(+), 21 deletions(-) create mode 100644 msg/SimulationState.msg create mode 100644 srv/GetSimulationState.srv delete mode 100644 srv/SetSimulationPaused.srv create mode 100644 srv/SetSimulationState.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 2558c33..96ceff5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,6 +24,7 @@ set(msg_files "msg/EntityState.msg" "msg/NamedPose.msg" "msg/Result.msg" + "msg/SimulationState.msg" "msg/SimulatorFeatures.msg" "msg/Spawnable.msg" "msg/TagsFilter.msg" @@ -38,11 +39,12 @@ set(srv_files "srv/GetEntityState.srv" "srv/GetNamedPoseBounds.srv" "srv/GetNamedPoses.srv" + "srv/GetSimulationState.srv" "srv/GetSimulatorFeatures.srv" "srv/GetSpawnables.srv" "srv/ResetSimulation.srv" "srv/SetEntityState.srv" - "srv/SetSimulationPaused.srv" + "srv/SetSimulationState.srv" "srv/SpawnEntity.srv" "srv/StepSimulation.srv" ) diff --git a/msg/EntityState.msg b/msg/EntityState.msg index a083753..5b805be 100644 --- a/msg/EntityState.msg +++ b/msg/EntityState.msg @@ -2,7 +2,7 @@ std_msgs/Header header # 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 body twist in the reference frame. - # It is observed and defined in the local coordinates system of the body. +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/SimulationState.msg b/msg/SimulationState.msg new file mode 100644 index 0000000..b197644 --- /dev/null +++ b/msg/SimulationState.msg @@ -0,0 +1,16 @@ +# Simulation states used in SetSimulationState and returned in GetSimulationState + +uint8 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 PLAYING = 1 # Simulation is playing, can be either paused or stopped. +uint8 PAUSED = 2 # Simulation is paused, can be either stopped (which will reset it) or played. +uint8 QUITTING = 3 # Closing the simulator application. Switching from PLAYING or 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 \ No newline at end of file diff --git a/srv/DeleteEntity.srv b/srv/DeleteEntity.srv index b837a03..e63dafe 100644 --- a/srv/DeleteEntity.srv +++ b/srv/DeleteEntity.srv @@ -1,6 +1,7 @@ -# Remove an entity (a robot, other object) by unique name from the simulation +# Remove an entity (a robot, other object) from the simulation -string name # Unique name with a namespace, as returned by SpawnEntity or GetEntities. +Entity entity # Entity identified by its unique name with a namespace, + # as returned by SpawnEntity or GetEntities. --- diff --git a/srv/GetEntityBounds.srv b/srv/GetEntityBounds.srv index 0ddc60e..c2242bd 100644 --- a/srv/GetEntityBounds.srv +++ b/srv/GetEntityBounds.srv @@ -1,6 +1,6 @@ # Get geometrical bounds for entity. This feature is available if GetSimulatorFeatures includes ENTITY_BOUNDS feature. -string name # Unique name as returned by GetEntities / SpawnEntity. +Entity entity # Entity identified by its unique name as returned by GetEntities / SpawnEntity. --- diff --git a/srv/GetEntityInfo.srv b/srv/GetEntityInfo.srv index 33c53e1..a048eaf 100644 --- a/srv/GetEntityInfo.srv +++ b/srv/GetEntityInfo.srv @@ -1,6 +1,6 @@ # Get type and other information about an entity. -string name # Unique name as returned by GetEntities / SpawnEntity. +Entity entity # Entity identified by its unique name as returned by GetEntities / SpawnEntity. --- diff --git a/srv/GetEntityState.srv b/srv/GetEntityState.srv index 16bc257..d498c0b 100644 --- a/srv/GetEntityState.srv +++ b/srv/GetEntityState.srv @@ -1,8 +1,8 @@ # Get state of an entity. -string name # Unique name as returned by GetEntities / SpawnEntity. +Entity 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. +EntityState state # Entity ground truth state. Valid when result.result = OK. diff --git a/srv/GetSimulationState.srv b/srv/GetSimulationState.srv new file mode 100644 index 0000000..b06c5b8 --- /dev/null +++ b/srv/GetSimulationState.srv @@ -0,0 +1,7 @@ +# Gets the simulation state (paused, playing, stopped) + +--- + +SimulationState state # Current state of the simulation. + +Result result diff --git a/srv/SetEntityState.srv b/srv/SetEntityState.srv index fe600cd..ff8bd8b 100644 --- a/srv/SetEntityState.srv +++ b/srv/SetEntityState.srv @@ -1,6 +1,6 @@ # Set a state of an object, which will result in an instant change in its pose and/or twist. -string name # Unique name as returned by GetEntities or SpawnEntity. +Entity 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. # Note that the acceleration field may be ignored by simulators. diff --git a/srv/SetSimulationPaused.srv b/srv/SetSimulationPaused.srv deleted file mode 100644 index bb8af48..0000000 --- a/srv/SetSimulationPaused.srv +++ /dev/null @@ -1,11 +0,0 @@ -# Pauses or unpauses the simulation - -bool pause # If true, sets simulation state to paused, otherwise sets it to running. - # If already in target state, nothing happens but the result informs about it. - ---- - -uint8 ALREADY_PAUSED = 101 # Additional result type for this call, which means simulation was already - # paused and remains so. - -Result result diff --git a/srv/SetSimulationState.srv b/srv/SetSimulationState.srv new file mode 100644 index 0000000..ac0f9c2 --- /dev/null +++ b/srv/SetSimulationState.srv @@ -0,0 +1,15 @@ +# Change the simulation state + + +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 STOP was requested). +uint8 STATE_TRANSITION_ERROR = 102 # The simulator failed to transition to the target state. This might happen + # especially with the transition to PLAYING from STOPPED. + # See result.error_message for details. +uint8 INCORRECT_TRANSITION = 103 # Incorrect transition (pausing when in stopped state). + +Result result From aee7e79e3a07923671b231b87e98d6dad6f8f7be Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Tue, 18 Feb 2025 14:00:28 +0100 Subject: [PATCH 24/33] Update msg/SimulationState.msg MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Michał Pełka --- msg/SimulationState.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/SimulationState.msg b/msg/SimulationState.msg index b197644..a6c62cd 100644 --- a/msg/SimulationState.msg +++ b/msg/SimulationState.msg @@ -13,4 +13,4 @@ uint8 QUITTING = 3 # Closing the simulator application. Swit # 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 \ No newline at end of file +uint8 state From 4497cbc73b317df6507cee519c1345929db05091 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Mon, 3 Mar 2025 12:40:15 +0100 Subject: [PATCH 25/33] Apply suggestions from code review Co-authored-by: David V. Lu!! --- msg/Bounds.msg | 12 ++++++------ msg/EntityState.msg | 2 +- msg/TagsFilter.msg | 6 +++--- srv/ResetSimulation.srv | 14 +++++++------- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/msg/Bounds.msg b/msg/Bounds.msg index eda04ba..6d3b58f 100644 --- a/msg/Bounds.msg +++ b/msg/Bounds.msg @@ -11,14 +11,14 @@ # before calling SpawnEntity, to avoid overlaps and unstable behavior. # bounds type -uint8 BOX = 0 # Bounding box, points field should have two values, which are - # upper right and lower left corners of the box. -uint8 CONVEX_HULL = 1 # Points define a convex hull (at least 3 non-collinear points). -uint8 SPHERE = 2 # 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 values are ignored). +uint8 TYPE_BOX = 0 # Bounding box, points field should have two values, which are + # upper right and lower left corners of the box. +uint8 TYPE_CONVEX_HULL = 1 # Points define a convex hull (at least 3 non-collinear points). +uint8 TYPE_SPHERE = 2 # 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 values are ignored). -uint8 type # See type value documentation. +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/EntityState.msg b/msg/EntityState.msg index 5b805be..c1a128d 100644 --- a/msg/EntityState.msg +++ b/msg/EntityState.msg @@ -1,6 +1,6 @@ # Entity current pose, twist and acceleration -std_msgs/Header header # Frame and timestamp for pose and twist. Empty frame defaults to world. +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 diff --git a/msg/TagsFilter.msg b/msg/TagsFilter.msg index 4a6a3d9..03c641e 100644 --- a/msg/TagsFilter.msg +++ b/msg/TagsFilter.msg @@ -4,7 +4,7 @@ string[] tags # Optional, defaults to empt # Results matching any or all of tags will be returned, depending # on tags_filter_mode. -uint8 ANY_OF = 0 # Filter with AND mode (all tags need to match). -uint8 ALL_OF = 1 # Filter with OR mode (any tag can match). +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 tags_filter_mode # Set to control filter application for tags. +uint8 filter_mode # Set to control filter application for tags. diff --git a/srv/ResetSimulation.srv b/srv/ResetSimulation.srv index 8c16b39..b763206 100644 --- a/srv/ResetSimulation.srv +++ b/srv/ResetSimulation.srv @@ -1,14 +1,14 @@ # Reset the simulation to the start, including the entire scene and the simulation time. # Objects that were dynamically spawned are de-spawned. -uint8 DEFAULT = 0 # same as ALL. -uint8 TIME = 1 # Reset simulation time to start. -uint8 STATE = 2 # Reset state such as poses and velocities. This may include state randomization - # if such feature is available and turned on. -uint8 SPAWNED = 4 # De-spawns all spawned entities. -uint8 ALL = 255 # Fully resets simulation to the start, as if it was closed and launched again. +uint8 SCOPE_DEFAULT = 0 # same as 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 reset_scope # Scope of the reset. Note that simulators might only support some types of resets. +uint8 scope # Scope of the reset. Note that simulators might only support some types of resets. # This is a bit field which may be checked for each scope e.g. reset_scope & TIME. --- From af3ebff1aeb2f8d430a44915c185396baacd42ed Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Mon, 3 Mar 2025 15:22:22 +0100 Subject: [PATCH 26/33] Applied review comments: - Entity message is removed as it was just a string. - Suggestions applied to usage of bounds. - Renaming of MultiStepSimulation to SimulateSteps. - Adjustments of SimulationFeatures to changes. - Multiple documentation clarifications / additions. Additionally, introduced TYPE_EMPTY for bounds for consistency. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- CMakeLists.txt | 3 +-- ...ltiStepSimulation.action => SimulateSteps.action} | 0 msg/Bounds.msg | 11 ++++++----- msg/Entity.msg | 4 ---- msg/EntityCategories.msg | 3 +-- msg/EntityFilters.msg | 9 ++++++--- msg/SimulatorFeatures.msg | 12 +++++++----- srv/DeleteEntity.srv | 2 +- srv/GetEntityBounds.srv | 4 +++- srv/GetEntityInfo.srv | 2 +- srv/GetEntityState.srv | 2 +- srv/GetNamedPoses.srv | 2 -- srv/ResetSimulation.srv | 4 ++-- srv/SetEntityState.srv | 2 +- srv/StepSimulation.srv | 2 +- 15 files changed, 31 insertions(+), 31 deletions(-) rename action/{MultiStepSimulation.action => SimulateSteps.action} (100%) delete mode 100644 msg/Entity.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 96ceff5..f0f703e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,7 +17,6 @@ find_package(std_msgs REQUIRED) set(msg_files "msg/Bounds.msg" - "msg/Entity.msg" "msg/EntityCategories.msg" "msg/EntityFilters.msg" "msg/EntityInfo.msg" @@ -50,7 +49,7 @@ set(srv_files ) set(action_files - "action/MultiStepSimulation.action" + "action/SimulateSteps.action" ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/action/MultiStepSimulation.action b/action/SimulateSteps.action similarity index 100% rename from action/MultiStepSimulation.action rename to action/SimulateSteps.action diff --git a/msg/Bounds.msg b/msg/Bounds.msg index 6d3b58f..c43d32d 100644 --- a/msg/Bounds.msg +++ b/msg/Bounds.msg @@ -5,17 +5,18 @@ # Bounds can be also checked to ensure certain scenario conditions are met. # For entities, these limits are relative to entity's top link transform, following ROS rep-103 convention. -# Empty bounds are to be understood as "unbounded". +# 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 area containing the (0,0,0) point which is the position. # 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_BOX = 0 # Bounding box, points field should have two values, which are +uint8 TYPE_EMPTY = 0 # No bounds. The points vector will be empty. +uint8 TYPE_BOX = 1 # Bounding box, points field should have two values, which are # upper right and lower left corners of the box. -uint8 TYPE_CONVEX_HULL = 1 # Points define a convex hull (at least 3 non-collinear points). -uint8 TYPE_SPHERE = 2 # 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 values are ignored). +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 diff --git a/msg/Entity.msg b/msg/Entity.msg deleted file mode 100644 index 10faee9..0000000 --- a/msg/Entity.msg +++ /dev/null @@ -1,4 +0,0 @@ -# Entity identified by its unique name. Entities are objects in the simulation such as models and links. -# Each simulator might define what an entity is in a (slightly) different way. - -string name # Entity unique name. diff --git a/msg/EntityCategories.msg b/msg/EntityCategories.msg index b1e29bb..9e13cf1 100644 --- a/msg/EntityCategories.msg +++ b/msg/EntityCategories.msg @@ -5,7 +5,6 @@ uint8 ROBOT = 1 # A broad category for mobile robots, arms, drones e uint8 HUMAN = 2 # Simulated humans, e.g. pedestrians, warehouse workers. Compared to DYNAMIC_OBJECT category, # humans are often expected to be treated exceptionally in regards to safety constraints. uint8 DYNAMIC_OBJECT = 4 # Vehicles, animals, mobile obstacles, typically to present a detection & tracking challenge, - # not themselves subject to validation. + # such as when simulation is used to test robot perception or navigation stack. uint8 STATIC_OBJECT = 5 # Any object which is static, e.g. not supposed to change its pose # unless by means of SetEntityState. - diff --git a/msg/EntityFilters.msg b/msg/EntityFilters.msg index 53d0f5a..6bd82d1 100644 --- a/msg/EntityFilters.msg +++ b/msg/EntityFilters.msg @@ -3,6 +3,8 @@ 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). uint8[] categories # Optional, defaults to empty, which means no category filter. # Entities matching any of the categories will be returned. @@ -11,7 +13,8 @@ uint8[] categories # Optional, defaults to empt TagsFilter tags # Tags filter to apply. To get entity tags, use GetEntityInfo. # Applies together with other filters (filter, categories). -# if overlap_radius > 0, the overlap filter is applied and entities overlapping with the defined sphere will be returned. +# 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. +# If service is called with filter bounds set to an unsupported type, a FEATURE_UNSUPPORTED result will be returned. -float64 overlap_radius # Radius of the overlap sphere -geometry_msgs/Point overlap_center # Center point of the overlap sphere, in world coordinates. +Bounds bounds diff --git a/msg/SimulatorFeatures.msg b/msg/SimulatorFeatures.msg index 4a62c09..2c0f9e1 100644 --- a/msg/SimulatorFeatures.msg +++ b/msg/SimulatorFeatures.msg @@ -4,8 +4,10 @@ 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_BOUNDS = 4 # Supports entity bounds (GetEntityBounds). -uint8 SPAWNING_RESOURCE_STRING = 5 # Supports SpawnEntity resource_string field. +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_CONVEX = 6 # Supports entity filtering with Bounds TYPE_CONVEX_HULL. +uint8 SPAWNING_RESOURCE_STRING = 7 # Supports SpawnEntity resource_string field. uint8 ENTITY_STATE_LISTING = 10 # Supports GetEntityState interface. uint8 ENTITY_STATE_SETTING = 11 # Supports SetEntityState interface. @@ -15,11 +17,11 @@ uint8 SIMULATION_RESET_TIME = 21 # Supports TIME flag for resetting. uint8 SIMULATION_RESET_STATE = 22 # Supports STATE flag for resetting. uint8 SIMULATION_RESET_SPAWNED = 23 # Supports SPAWNED flag for resetting. -uint8 SIMULATION_PAUSE = 30 # Supports SetSimulationPaused interface +uint8 SIMULATION_PAUSE = 30 # Supports the 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 MultiStepSimulation action. -uint8 STEP_SIMULATION_ACTION = 33 # Supports MultiStepSimulation action interface. + # 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. diff --git a/srv/DeleteEntity.srv b/srv/DeleteEntity.srv index e63dafe..aa2df2d 100644 --- a/srv/DeleteEntity.srv +++ b/srv/DeleteEntity.srv @@ -1,6 +1,6 @@ # Remove an entity (a robot, other object) from the simulation -Entity entity # Entity identified by its unique name with a namespace, +string entity # Entity identified by its unique name with a namespace, # as returned by SpawnEntity or GetEntities. --- diff --git a/srv/GetEntityBounds.srv b/srv/GetEntityBounds.srv index c2242bd..51c6404 100644 --- a/srv/GetEntityBounds.srv +++ b/srv/GetEntityBounds.srv @@ -1,8 +1,10 @@ # Get geometrical bounds for entity. This feature is available if GetSimulatorFeatures includes ENTITY_BOUNDS feature. -Entity entity # Entity identified by its unique name as returned by GetEntities / SpawnEntity. +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 index a048eaf..1a4b719 100644 --- a/srv/GetEntityInfo.srv +++ b/srv/GetEntityInfo.srv @@ -1,6 +1,6 @@ # Get type and other information about an entity. -Entity entity # Entity identified by its unique name as returned by GetEntities / SpawnEntity. +string entity # Entity identified by its unique name as returned by GetEntities / SpawnEntity. --- diff --git a/srv/GetEntityState.srv b/srv/GetEntityState.srv index d498c0b..4edcfe4 100644 --- a/srv/GetEntityState.srv +++ b/srv/GetEntityState.srv @@ -1,6 +1,6 @@ # Get state of an entity. -Entity entity # Entity identified by its unique name as returned by GetEntities / SpawnEntity. +string entity # Entity identified by its unique name as returned by GetEntities / SpawnEntity. --- diff --git a/srv/GetNamedPoses.srv b/srv/GetNamedPoses.srv index f026eaa..cd17780 100644 --- a/srv/GetNamedPoses.srv +++ b/srv/GetNamedPoses.srv @@ -1,6 +1,4 @@ # Get predefined poses which are convenient to for spawning, navigation goals etc. -# This is an optional simulation feature: check status_message if you receive an empty poses vector, -# to determine whether this feature is not supported by your simulator, or named poses are simply not defined. TagsFilter tags # Tags filter to apply. Only named poses with matching tags field # will be returned. Can be empty (see TagsFilter). diff --git a/srv/ResetSimulation.srv b/srv/ResetSimulation.srv index b763206..68a904d 100644 --- a/srv/ResetSimulation.srv +++ b/srv/ResetSimulation.srv @@ -8,8 +8,8 @@ uint8 SCOPE_STATE = 2 # Reset state such as poses and veloci 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 types of resets. - # This is a bit field which may be checked for each scope e.g. reset_scope & TIME. +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 & TIME. --- diff --git a/srv/SetEntityState.srv b/srv/SetEntityState.srv index ff8bd8b..dbde080 100644 --- a/srv/SetEntityState.srv +++ b/srv/SetEntityState.srv @@ -1,6 +1,6 @@ # Set a state of an object, which will result in an instant change in its pose and/or twist. -Entity entity # Entity identified by its unique name as returned by GetEntities / SpawnEntity. +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. # Note that the acceleration field may be ignored by simulators. diff --git a/srv/StepSimulation.srv b/srv/StepSimulation.srv index 423f7a6..1bf331a 100644 --- a/srv/StepSimulation.srv +++ b/srv/StepSimulation.srv @@ -1,6 +1,6 @@ # 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 MultiStepSimulation action, which is worth considering for a multi-step interface. +# There is an alternative in SimulateSteps action, which is worth considering for a multi-step interface. uint64 steps # Step through the simulation loop this many times. From 5f0aefb967ef9eaa01ca860b3c2ab6c684bb7036 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Tue, 4 Mar 2025 17:13:39 +0100 Subject: [PATCH 27/33] corrected entities to strings MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- srv/GetEntities.srv | 2 +- srv/GetEntitiesStates.srv | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/srv/GetEntities.srv b/srv/GetEntities.srv index dc96012..3cbf707 100644 --- a/srv/GetEntities.srv +++ b/srv/GetEntities.srv @@ -8,4 +8,4 @@ EntityFilters filters # Optional filters for the q --- Result result -Entity[] entities # All entities matching the filters. +string[] entities # Unique names of all entities matching the filters. diff --git a/srv/GetEntitiesStates.srv b/srv/GetEntitiesStates.srv index dddf860..444eda9 100644 --- a/srv/GetEntitiesStates.srv +++ b/srv/GetEntitiesStates.srv @@ -7,5 +7,5 @@ EntityFilters filters # Optional filters for the q --- Result result -Entity[] entities # All entities matching the filters. +string[] entities # Unique names of all entities matching the filters. EntityState[] states # States for these entities. From 5fe0d30c41ed38116beff44db85d5611e6309524 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Mon, 17 Mar 2025 11:35:30 +0100 Subject: [PATCH 28/33] Update msg/Bounds.msg Co-authored-by: David V. Lu!! --- msg/Bounds.msg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/msg/Bounds.msg b/msg/Bounds.msg index c43d32d..db3692c 100644 --- a/msg/Bounds.msg +++ b/msg/Bounds.msg @@ -12,8 +12,8 @@ # bounds type uint8 TYPE_EMPTY = 0 # No bounds. The points vector will be empty. -uint8 TYPE_BOX = 1 # Bounding box, points field should have two values, which are - # upper right and lower left corners of the box. +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). From eb6f27e20c65b58dc8c45691934395171f14c703 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Mon, 17 Mar 2025 13:22:30 +0100 Subject: [PATCH 29/33] Applied review - Added SetEntityInfo to be enable setting of items such as tags. - EntityCategory is now self-contained. - Updated SimulationFeatures to reflect changes - Adjusted documentation in a few places. - Added 2 new sections to the README. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- CMakeLists.txt | 3 ++- README.md | 21 +++++++++++++++++++ msg/Bounds.msg | 2 +- ...ntityCategories.msg => EntityCategory.msg} | 5 ++++- msg/EntityFilters.msg | 2 +- msg/EntityInfo.msg | 3 +-- msg/SimulatorFeatures.msg | 1 + srv/GetEntityInfo.srv | 2 +- srv/GetNamedPoseBounds.srv | 2 +- srv/SetEntityInfo.srv | 8 +++++++ 10 files changed, 41 insertions(+), 8 deletions(-) rename msg/{EntityCategories.msg => EntityCategory.msg} (81%) create mode 100644 srv/SetEntityInfo.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index f0f703e..f9f6b0d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,7 +17,7 @@ find_package(std_msgs REQUIRED) set(msg_files "msg/Bounds.msg" - "msg/EntityCategories.msg" + "msg/EntityCategory.msg" "msg/EntityFilters.msg" "msg/EntityInfo.msg" "msg/EntityState.msg" @@ -42,6 +42,7 @@ set(srv_files "srv/GetSimulatorFeatures.srv" "srv/GetSpawnables.srv" "srv/ResetSimulation.srv" + "srv/SetEntityInfo.srv" "srv/SetEntityState.srv" "srv/SetSimulationState.srv" "srv/SpawnEntity.srv" diff --git a/README.md b/README.md index 243cb19..55c9481 100644 --- a/README.md +++ b/README.md @@ -2,3 +2,24 @@ 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/msg/Bounds.msg b/msg/Bounds.msg index db3692c..012ec2c 100644 --- a/msg/Bounds.msg +++ b/msg/Bounds.msg @@ -6,7 +6,7 @@ # For entities, these limits are relative to entity's top 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 area containing the (0,0,0) point which is the position. +# 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. diff --git a/msg/EntityCategories.msg b/msg/EntityCategory.msg similarity index 81% rename from msg/EntityCategories.msg rename to msg/EntityCategory.msg index 9e13cf1..d4806c7 100644 --- a/msg/EntityCategories.msg +++ b/msg/EntityCategory.msg @@ -1,4 +1,5 @@ -# Constants for entity categories +# 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 OBJECT = 0 # Generic or unspecified type. uint8 ROBOT = 1 # A broad category for mobile robots, arms, drones etc., usually with ROS 2 interfaces. @@ -8,3 +9,5 @@ uint8 DYNAMIC_OBJECT = 4 # Vehicles, animals, mobile obstacles, typically to # such as when simulation is used to test robot perception or navigation stack. uint8 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 index 6bd82d1..7a4747a 100644 --- a/msg/EntityFilters.msg +++ b/msg/EntityFilters.msg @@ -6,7 +6,7 @@ string filter # Optional, defaults to empt # The regular expression syntax is POSIX Extended, # see https://pubs.opengroup.org/onlinepubs/9799919799/ definitions. # Applies together with other filters (categories, tags). -uint8[] categories # Optional, defaults to empty, which means no category filter. +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). diff --git a/msg/EntityInfo.msg b/msg/EntityInfo.msg index 4a4c526..eeb7fdc 100644 --- a/msg/EntityInfo.msg +++ b/msg/EntityInfo.msg @@ -1,6 +1,5 @@ # Entity type and additional information -uint8 category # Major category for the entity. Extra entity type distinction can be made through tags. - # See EntityCategories for defined category values. +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/SimulatorFeatures.msg b/msg/SimulatorFeatures.msg index 2c0f9e1..6d2f8ed 100644 --- a/msg/SimulatorFeatures.msg +++ b/msg/SimulatorFeatures.msg @@ -11,6 +11,7 @@ uint8 SPAWNING_RESOURCE_STRING = 7 # Supports SpawnEntity resource_string uint8 ENTITY_STATE_LISTING = 10 # Supports GetEntityState interface. uint8 ENTITY_STATE_SETTING = 11 # Supports SetEntityState interface. +uint8 ENTITY_INFO_SETTING = 12 # Supports SetEntityInfo interface. uint8 SIMULATION_RESET = 20 # Supports one or more ways to reset the simulation through ResetSimulation. uint8 SIMULATION_RESET_TIME = 21 # Supports TIME flag for resetting. diff --git a/srv/GetEntityInfo.srv b/srv/GetEntityInfo.srv index 1a4b719..0ac0189 100644 --- a/srv/GetEntityInfo.srv +++ b/srv/GetEntityInfo.srv @@ -1,6 +1,6 @@ # Get type and other information about an entity. -string entity # Entity identified by its unique name as returned by GetEntities / SpawnEntity. +string entity # Entity identified by its unique name as returned by GetEntities. --- diff --git a/srv/GetNamedPoseBounds.srv b/srv/GetNamedPoseBounds.srv index a30d515..c92bcda 100644 --- a/srv/GetNamedPoseBounds.srv +++ b/srv/GetNamedPoseBounds.srv @@ -1,6 +1,6 @@ # Get bounds for the named pose. This feature is available if GetSimulatorFeatures includes POSE_BOUNDS feature. -string name # unique names (as returned from GetNamedPoses). +string name # unique name (as returned from GetNamedPoses). --- diff --git a/srv/SetEntityInfo.srv b/srv/SetEntityInfo.srv new file mode 100644 index 0000000..1891c04 --- /dev/null +++ b/srv/SetEntityInfo.srv @@ -0,0 +1,8 @@ +# Set type, tags and other information about an entity. + +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 From e98721fdd72b58674e2d5b6e8052afeaf23fc05d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Tue, 18 Mar 2025 11:38:31 +0100 Subject: [PATCH 30/33] Applied review - Documentation of relevant feature support flag per each interface. - Enumeration naming changed to TYPE_, CATEGORY_ etc. - Default to 1 for steps. - Other minor suggested docs changes. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- action/SimulateSteps.action | 1 + msg/Bounds.msg | 2 +- msg/EntityCategory.msg | 19 +++++++++------- msg/EntityFilters.msg | 17 +++++++++----- msg/Result.msg | 28 +++++++++++------------ msg/SimulationState.msg | 10 ++++----- msg/SimulatorFeatures.msg | 45 +++++++++++++++++++++---------------- msg/TagsFilter.msg | 6 ++--- srv/DeleteEntity.srv | 3 ++- srv/GetEntitiesStates.srv | 1 + srv/GetEntityBounds.srv | 1 + srv/GetEntityInfo.srv | 1 + srv/GetEntityState.srv | 1 + srv/GetNamedPoseBounds.srv | 1 + srv/GetNamedPoses.srv | 1 + srv/GetSimulationState.srv | 1 + srv/GetSpawnables.srv | 1 + srv/ResetSimulation.srv | 6 +++-- srv/SetEntityInfo.srv | 1 + srv/SetEntityState.srv | 1 + srv/SetSimulationState.srv | 9 ++++---- srv/SpawnEntity.srv | 4 +++- srv/StepSimulation.srv | 6 +++-- 23 files changed, 100 insertions(+), 66 deletions(-) diff --git a/action/SimulateSteps.action b/action/SimulateSteps.action index 572a5bb..4ba9ab7 100644 --- a/action/SimulateSteps.action +++ b/action/SimulateSteps.action @@ -1,6 +1,7 @@ # 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. --- diff --git a/msg/Bounds.msg b/msg/Bounds.msg index 012ec2c..ed16b89 100644 --- a/msg/Bounds.msg +++ b/msg/Bounds.msg @@ -3,7 +3,7 @@ # 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 top link transform, following ROS rep-103 convention. +# 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. diff --git a/msg/EntityCategory.msg b/msg/EntityCategory.msg index d4806c7..63d3c0f 100644 --- a/msg/EntityCategory.msg +++ b/msg/EntityCategory.msg @@ -1,13 +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 OBJECT = 0 # Generic or unspecified type. -uint8 ROBOT = 1 # A broad category for mobile robots, arms, drones etc., usually with ROS 2 interfaces. -uint8 HUMAN = 2 # Simulated humans, e.g. pedestrians, warehouse workers. Compared to DYNAMIC_OBJECT category, - # humans are often expected to be treated exceptionally in regards to safety constraints. -uint8 DYNAMIC_OBJECT = 4 # Vehicles, animals, mobile obstacles, typically to present a detection & tracking challenge, - # such as when simulation is used to test robot perception or navigation stack. -uint8 STATIC_OBJECT = 5 # Any object which is static, e.g. not supposed to change its pose - # unless by means of SetEntityState. +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 index 7a4747a..967ffac 100644 --- a/msg/EntityFilters.msg +++ b/msg/EntityFilters.msg @@ -10,11 +10,16 @@ EntityCategory[] categories # Optional, defaults to empt # 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). - -# 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. -# If service is called with filter bounds set to an unsupported type, a FEATURE_UNSUPPORTED result will be returned. - -Bounds bounds + # 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/Result.msg b/msg/Result.msg index 7483551..64f3879 100644 --- a/msg/Result.msg +++ b/msg/Result.msg @@ -1,18 +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 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 OK = 1 -uint8 NOT_FOUND = 2 # No match for input (such as when entity name does not exist). -uint8 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 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_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. +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 index a6c62cd..41e3942 100644 --- a/msg/SimulationState.msg +++ b/msg/SimulationState.msg @@ -1,14 +1,14 @@ # Simulation states used in SetSimulationState and returned in GetSimulationState -uint8 STOPPED = 0 # Simulation is stopped, which is equivalent to pausing and resetting with ALL. +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 PLAYING = 1 # Simulation is playing, can be either paused or stopped. -uint8 PAUSED = 2 # Simulation is paused, can be either stopped (which will reset it) or played. -uint8 QUITTING = 3 # Closing the simulator application. Switching from PLAYING or PAUSED states - # is expected to stop the simulation first, and then exit. +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. diff --git a/msg/SimulatorFeatures.msg b/msg/SimulatorFeatures.msg index 6d2f8ed..023c80d 100644 --- a/msg/SimulatorFeatures.msg +++ b/msg/SimulatorFeatures.msg @@ -1,31 +1,38 @@ # 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_CONVEX = 6 # Supports entity filtering with Bounds TYPE_CONVEX_HULL. -uint8 SPAWNING_RESOURCE_STRING = 7 # Supports SpawnEntity resource_string field. +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_LISTING = 10 # Supports GetEntityState interface. +uint8 ENTITY_STATE_GETTING = 10 # Supports GetEntityState interface. uint8 ENTITY_STATE_SETTING = 11 # Supports SetEntityState interface. -uint8 ENTITY_INFO_SETTING = 12 # Supports SetEntityInfo 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 TIME flag for resetting. -uint8 SIMULATION_RESET_STATE = 22 # Supports STATE flag for resetting. -uint8 SIMULATION_RESET_SPAWNED = 23 # Supports SPAWNED flag for resetting. +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 SIMULATION_PAUSE = 30 # Supports the 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. +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. +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) diff --git a/msg/TagsFilter.msg b/msg/TagsFilter.msg index 03c641e..5d087f7 100644 --- a/msg/TagsFilter.msg +++ b/msg/TagsFilter.msg @@ -1,8 +1,8 @@ # 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. +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). diff --git a/srv/DeleteEntity.srv b/srv/DeleteEntity.srv index aa2df2d..334c92b 100644 --- a/srv/DeleteEntity.srv +++ b/srv/DeleteEntity.srv @@ -1,4 +1,5 @@ -# Remove an entity (a robot, other object) from the simulation +# 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. diff --git a/srv/GetEntitiesStates.srv b/srv/GetEntitiesStates.srv index 444eda9..069f5cb 100644 --- a/srv/GetEntitiesStates.srv +++ b/srv/GetEntitiesStates.srv @@ -1,5 +1,6 @@ # 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_LISTING value in GetSimulationFeatures. EntityFilters filters # Optional filters for the query, including name, category, tags, # and overlap filters. diff --git a/srv/GetEntityBounds.srv b/srv/GetEntityBounds.srv index 51c6404..3c36640 100644 --- a/srv/GetEntityBounds.srv +++ b/srv/GetEntityBounds.srv @@ -1,4 +1,5 @@ # 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. diff --git a/srv/GetEntityInfo.srv b/srv/GetEntityInfo.srv index 0ac0189..09370bc 100644 --- a/srv/GetEntityInfo.srv +++ b/srv/GetEntityInfo.srv @@ -1,4 +1,5 @@ # 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. diff --git a/srv/GetEntityState.srv b/srv/GetEntityState.srv index 4edcfe4..34f88d1 100644 --- a/srv/GetEntityState.srv +++ b/srv/GetEntityState.srv @@ -1,4 +1,5 @@ # Get state of an entity. +# Support for this interface is indicated through the ENTITY_STATE_LISTING value in GetSimulationFeatures. string entity # Entity identified by its unique name as returned by GetEntities / SpawnEntity. diff --git a/srv/GetNamedPoseBounds.srv b/srv/GetNamedPoseBounds.srv index c92bcda..088b9d3 100644 --- a/srv/GetNamedPoseBounds.srv +++ b/srv/GetNamedPoseBounds.srv @@ -1,4 +1,5 @@ # 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). diff --git a/srv/GetNamedPoses.srv b/srv/GetNamedPoses.srv index cd17780..cb9158c 100644 --- a/srv/GetNamedPoses.srv +++ b/srv/GetNamedPoses.srv @@ -1,4 +1,5 @@ # 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). diff --git a/srv/GetSimulationState.srv b/srv/GetSimulationState.srv index b06c5b8..9b4cd2c 100644 --- a/srv/GetSimulationState.srv +++ b/srv/GetSimulationState.srv @@ -1,4 +1,5 @@ # Gets the simulation state (paused, playing, stopped) +# Support for this interface is indicated through the SIMULATION_STATE_GETTING value in GetSimulationFeatures. --- diff --git a/srv/GetSpawnables.srv b/srv/GetSpawnables.srv index 0009847..ad18d0f 100644 --- a/srv/GetSpawnables.srv +++ b/srv/GetSpawnables.srv @@ -1,5 +1,6 @@ # 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 diff --git a/srv/ResetSimulation.srv b/srv/ResetSimulation.srv index 68a904d..a53afd5 100644 --- a/srv/ResetSimulation.srv +++ b/srv/ResetSimulation.srv @@ -1,7 +1,9 @@ # 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 ALL. +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. @@ -9,7 +11,7 @@ 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 & TIME. + # This is a bit field which may be checked for each scope e.g. scope & SCOPE_TIME. --- diff --git a/srv/SetEntityInfo.srv b/srv/SetEntityInfo.srv index 1891c04..d77561e 100644 --- a/srv/SetEntityInfo.srv +++ b/srv/SetEntityInfo.srv @@ -1,4 +1,5 @@ # 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. diff --git a/srv/SetEntityState.srv b/srv/SetEntityState.srv index dbde080..f738d32 100644 --- a/srv/SetEntityState.srv +++ b/srv/SetEntityState.srv @@ -1,4 +1,5 @@ # 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. diff --git a/srv/SetSimulationState.srv b/srv/SetSimulationState.srv index ac0f9c2..e5a9604 100644 --- a/srv/SetSimulationState.srv +++ b/srv/SetSimulationState.srv @@ -1,14 +1,15 @@ # 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 STOP was requested). +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 PLAYING from STOPPED. + # 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). diff --git a/srv/SpawnEntity.srv b/srv/SpawnEntity.srv index 9dbfe67..0e76c10 100644 --- a/srv/SpawnEntity.srv +++ b/srv/SpawnEntity.srv @@ -1,4 +1,5 @@ # 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, @@ -16,7 +17,8 @@ string uri # Resource such as SDFormat, URDF, USD o # 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). - # Check whether it is supported by your simulator through GetSimulatorFeatures. + # 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. diff --git a/srv/StepSimulation.srv b/srv/StepSimulation.srv index 1bf331a..6ec1376 100644 --- a/srv/StepSimulation.srv +++ b/srv/StepSimulation.srv @@ -1,9 +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 # Step through the simulation loop this many times. +uint64 steps 1 # Step through the simulation loop this many times. --- -Result result # Calling with simulation not paused will return OPERATION_FAILED and error message. +Result result # Calling with simulation not paused will return RESULT_OPERATION_FAILED and error message. From 65a653fabb5e86c75b652bab29eb5e279e258369 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Tue, 18 Mar 2025 13:06:23 +0100 Subject: [PATCH 31/33] feature naming fix MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Adam Dąbrowski --- srv/GetEntitiesStates.srv | 2 +- srv/GetEntityState.srv | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/srv/GetEntitiesStates.srv b/srv/GetEntitiesStates.srv index 069f5cb..1e18846 100644 --- a/srv/GetEntitiesStates.srv +++ b/srv/GetEntitiesStates.srv @@ -1,6 +1,6 @@ # 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_LISTING value in GetSimulationFeatures. +# 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. diff --git a/srv/GetEntityState.srv b/srv/GetEntityState.srv index 34f88d1..b054ec2 100644 --- a/srv/GetEntityState.srv +++ b/srv/GetEntityState.srv @@ -1,5 +1,5 @@ # Get state of an entity. -# Support for this interface is indicated through the ENTITY_STATE_LISTING value in GetSimulationFeatures. +# 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. From 4e6867d945a1de5b544c78014e07b48799eff7d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Thu, 20 Mar 2025 13:02:41 +0100 Subject: [PATCH 32/33] Update srv/SetEntityState.srv MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Paweł Liberadzki --- srv/SetEntityState.srv | 3 +++ 1 file changed, 3 insertions(+) diff --git a/srv/SetEntityState.srv b/srv/SetEntityState.srv index f738d32..1942917 100644 --- a/srv/SetEntityState.srv +++ b/srv/SetEntityState.srv @@ -3,6 +3,9 @@ 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. # Note that the acceleration field may be ignored by simulators. --- From 2206c58eabb562193b2b7d6bf5c368544b9f1fda Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Thu, 20 Mar 2025 19:27:38 +0100 Subject: [PATCH 33/33] Update srv/SetEntityState.srv MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Paweł Liberadzki --- srv/SetEntityState.srv | 1 - 1 file changed, 1 deletion(-) diff --git a/srv/SetEntityState.srv b/srv/SetEntityState.srv index 1942917..51063f4 100644 --- a/srv/SetEntityState.srv +++ b/srv/SetEntityState.srv @@ -6,7 +6,6 @@ EntityState state # New state to set immediately. The time # 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. - # Note that the acceleration field may be ignored by simulators. ---