diff --git a/rep.md b/rep.md index a2dec59..5dda686 100644 --- a/rep.md +++ b/rep.md @@ -19,10 +19,11 @@ This REP defines a standard schema and strict profile of OpenUSD (Universal Scen 2. **Runtime integrations** (ROS Interfaces). 3. **Converters and web visualization** (especially glTF 2.0 conversion). -To achieve this, the specification addresses three key areas: +To achieve this, the specification addresses four key areas: * **Section 1** adopts existing upstream standards and recommendations (AOUSD, ASWF, NVIDIA) to establish a baseline for correct simulation assets. * **Section 2** defines novel, declarative API schemas for ROS interfaces to ensure engine-agnostic runtime behavior. * **Section 3** defines a strict interoperability profile to support export pathways to other formats, ensuring compatibility with standards like glTF 2.0. +* **Section 4** defines schemas for robot control, covering both low-level joint identification and high-level application-level controllers. ## Motivation @@ -176,7 +177,7 @@ The root prim of a ROS-interfaced simulation asset may define its context namesp ROS interface schemas (`RosTopicAPI`, `RosServiceAPI`, `RosActionAPI`) must be applied to prims according to these placement rules: -* **Robot-wide interfaces:** Aggregate interfaces spanning a kinematic tree (e.g., `JointState` publisher, `FollowJointTrajectory` server) must be placed on or directly beneath the prim bearing the `RosContextAPI`. +* **Robot-wide interfaces:** Aggregate interfaces spanning a kinematic tree (e.g., `JointState` publisher, `FollowJointTrajectory` server) should be placed at root prim of the robot assembly. * **Sensor interfaces:** Localized interfaces (e.g., `Image`, `LaserScan`) must be placed on a child `UsdGeomXform` of the physical link. Multiple interfaces for the same sensor (e.g., `image_raw` and `camera_info`) must distribute them across separate child prims, one interface per prim. * **Interface prims must reside outside payloads.** Prims bearing `Ros*API` schemas are part of the lightweight kinematic/interface graph and must be traversable without loading geometry payloads. @@ -234,7 +235,12 @@ Note: The broadcast frequency of TF frames is an implementation detail left to t ### 2.8 Optical Frames OpenUSD cameras natively face the -Z axis, whereas ROS optical frames (REP 103) must face +Z. To bridge this without opaque simulator-side rotations, authors must decouple the physical sensor from its optical interface. Authors must create a child UsdGeomXform (e.g., `camera_optical_frame`) rotated 180 degrees around its local X-axis. All RosTopicAPI and RosFrameAPI schemas must be applied exclusively to this optical frame, ensuring deterministic data orientation in RViz. ---- +### 2.9 Custom names to ROS joints. + +Number of concepts in ROS (e.g. robot descriptions, controllers) relly on joints names. +To ensure that joints are correctly identified and mapped to said concepts, the custom property `ros:joint:name` must be applied to all Prims bearing built-in`UsdPhysicsJoint` schema. +This string value is source of joint name for all ROS communications (e.g., `FollowJointTrajectory` action goals, `JointState` messages), intergration with ROS tools (e.g., `ros2_control`), and mapping to other formats (e.g., MJCF's ``). +If this property is missing, simulators must fall back to using the prim name. ## 3. Export and Conversion @@ -288,6 +294,122 @@ OpenUSD and robotics XML formats (URDF, SDF, MJCF) are fundamentally mismatched * **API Translation:** Ros*API schemas must map exclusively to modern extension blocks (e.g., SDF ``, MJCF ``). Obsolete approaches such as injecting legacy `` tags into URDF are not allowed. * **Discard, not inject:** OpenUSD-native metadata (layer stacks, unselected variants) must be cleanly discarded. Injecting custom, non-standard XML elements to store unmappable OpenUSD states is not recommended. If pipeline necessitates it for practicality, such metadata must be confined to valid, format-native extension points. +## 4. Robot Control + +In robotic simulation there are two approaches to simulating a robotic system — application level and control level simulation. + +In control level simulation, the focus is on the individual components. +This approach is to be used for advanced use cases or validating low level robot controllers. +In this case the simulator is to expose joint state and command interface to control algorithm using API. +Good example of such API is `hardware_interface` in `ros2_control` package. +The REP-XXXX does not propose shape of the API and interface, and simulators are to build and maintain compatibility with external control frameworks, +it can be both ROS communication, inter-process communication, a shared library loaded by a simulator process or hardware-in-the loop solution (e.g. CAN bus link). + +In application level simulation, the simulator simulates the robot as a whole, including its physical properties, kinematics, and dynamics. +This approach is meant to be used for application testing (e.g. whole robotics stacks, mapping, localization frameworks). +In this approach the controller is integrated in the simulator codebase and managed by parameters of the prim and ROS communication. + +### 4.1 External Control Interfaces + +The ROS 2 external control `ROSExternalControlAPI` is a schema for exposing robot control interfaces to external control algorithms. +This schema is to be included as a built-in schema via `prepend apiSchemas` by a simulator-specific interface for controller. +Simulator loading prim with this schema should establish connection, load controller plugin, spawn a controller instance or set up hardware-in-the-loop connection to the robot controller, +and expose the control and state interfaces to control entity. + +### 4.2 Integrated Controller Simulation + +`ROSIntegratedControlAPI` allows instantiating robot controllers directly in the simulated scene. +It must reference one or more prims that have [ROSTopicAPI](#24-topic-interface-rostopicapi), [ROSServiceAPI](#25-service-interface-rosserviceapi) or [ROSActionAPI](#26-action-interface-rosactionapi). +This schema is to be included as a built-in schema via `prepend apiSchemas` by a simulator-specific controller schema in the simulator. +Example can be `simulatorXYZ::CustomROSRigidBodyTwistControllerAPI` which will include as built-in `ROSControlAPI` and reference prims: +- `ROSTopicAPI` for subscription of control message. +- `PhysicsRigidBodyAPI` for interaction with the physics engine. + +USD does not enforce API schema constraints on referenced prims at the schema definition level. It is the responsibility of the simulator to validate that all prims referenced by ROSControlAPI have at least one of the following API schemas applied: `ROSTopicAPI`, `ROSServiceAPI` or `ROSActionAPI`. + +### 4.2.1 Built-in Controllers + +The following schemas are built-in controller schemas that include +`ROSControlAPI` as a built-in via `prepend apiSchemas`. +Simulators may implement these schemas to provide a standardized +control interface for common use cases. +The following controllers are proposed as minimal for initial compliance. +The parameters and logic should follow established controllers in the ROS ecosystem and allow bootstrapping robot simulation with minimal custom development against +the typical use cases. +Simulators may choose to implement additional controllers as needed for their specific use cases and robot types, but these three are proposed as a baseline for compliance for +application level simulation. +The implementation should allow performing multi-robot simulation and control by leveraging the namespaces. + +#### 4.2.1.1 ROSRigidBodyTwistControllerAPI + +Controls a rigid body by subscribing to a topic with type `geometry_msgs/Twist` +and applying the commanded linear and angular velocities directly to the robot's body +with optional acceleration and velocity limits. +Implementation should follow logic similar to the `diff_drive_controller` in `ros2_controllers` package. + +- `rel ros:rigid_body_twist:subscriber`: Reference to a prim with `ROSTopicAPI` + for subscribing to twist control messages. +- `rel ros:rigid_body_twist:body`: Reference to a prim with + `UsdPhysicsRigidBodyAPI` for applying velocities. + +- `double ros:rigid_body_twist:cmd_vel_timeout`: Timeout in seconds after + which the command is considered stale. Default: `0.5`. +- `double ros:rigid_body_twist:linear:x:max_velocity`: Maximum linear velocity in m/s. +- `double ros:rigid_body_twist:linear:x:max_acceleration`: Maximum linear acceleration in m/s². +- `double ros:rigid_body_twist:angular:z:max_velocity`: Maximum angular velocity in rad/s. +- `double ros:rigid_body_twist:angular:z:max_acceleration`: Maximum angular acceleration in rad/s². + +#### 4.2.1.2 ROSDiffDriveControllerAPI + +Controls a differential drive robot by subscribing to a topic with type +`geometry_msgs/Twist` message and converting the commanded linear +and angular velocities into individual wheel velocities applied +to the simulator joints. +The computed velocities should be applied to prims with `UsdPhysicsDriveAPI` (part of the native `USDPhysics`) representing the wheel joints, +and the controller should publish odometry data to a topic with type `nav_msgs/Odometry`. +Implementation should follow logic similar to the `diff_drive_controller` in `ros2_controllers` package. + +- `rel ros:diff_drive:subscriber`: Reference to prim with `ROSTopicAPI` for subscribing to velocity commands. +- `rel ros:diff_drive:odom`: Reference to prim with `ROSTopicAPI` for publishing odometry data. +- `rel[] ros:diff_drive:left_wheels`: References to prims with `UsdPhysicsDriveAPI` representing left wheel joints. +- `rel[] ros:diff_drive:right_wheels`: References to prims with `UsdPhysicsDriveAPI` representing right wheel joints. +- `double ros:diff_drive:wheel_separation`: Distance between left and right wheels in meters. +- `double ros:diff_drive:wheel_radius`: Radius of the wheels in meters. +- `double ros:diff_drive:cmd_vel_timeout`: Timeout in seconds after which the command is considered stale. Default: `0.5`. +- `double ros:diff_drive:linear:x:max_velocity`: Maximum linear velocity in m/s. +- `double ros:diff_drive:linear:x:max_acceleration`: Maximum linear acceleration in m/s². +- `double ros:diff_drive:angular:z:max_velocity`: Maximum angular velocity in rad/s. +- `double ros:diff_drive:angular:z:max_acceleration`: Maximum angular acceleration in rad/s². + +#### 4.2.1.3 ROSJointTrajectoryControllerAPI + +Executes a joint trajectory by accepting a `control_msgs/FollowJointTrajectory` action goal and commanding +the simulator to follow the specified trajectory. +Implementation should follow logic similar to the `joint_trajectory_controller` in `ros2_controllers` package. +Implementation needs to check the name for [custom joint names](rep.md#29-custom-names-to-ros-joints) in `ros:joint:name` property of the joint prims and +use it for mapping trajectory points to joints in the simulator. + +- `rel ros:joint_trajectory:server`: Reference to a prim with `ROSActionAPI` + for receiving trajectory action goals. +- `rel[] ros:joint_trajectory:command_joints`: References to prims with `UsdPhysicsJointAPI`. +- `double ros:joint_trajectory:action_monitor_rate`: Frequency in Hz for + monitoring trajectory execution progress. +- `double ros:joint_trajectory:stopped_velocity_tolerance`: Velocity tolerance + at the end of the trajectory that indicates the controlled system has stopped. +- `double ros:joint_trajectory:timeout`: Maximum time allowed to reach the trajectory goal. + +#### 4.2.1.4 ROSJointStateBroadcasterAPI + +Reads joint states from the simulator and publishes them as `sensor_msgs/JointState` messages to a ROS topic. +Implementation should follow logic similar to the `joint_state_broadcaster` in `ros2_controllers` package. +Implementation needs to check the name for [custom joint names](rep.md#29-custom-names-to-ros-joints) in `ros:joint:name` +property of the joint prims and use it for mapping trajectory points to joints in the simulator. + +- `rel ros:joint_state_broadcaster:publisher`: Reference to a prim with `ROSTopicAPI` for publishing joint state data. +- `rel[] ros:joint_state_broadcaster:joints`: References to prims with `UsdPhysicsJointAPI` representing the joints whose states are to be broadcast. +- `double ros:joint_state_broadcaster:publish_rate`: Frequency in Hz at which joint states are published. +- `string ros:joint_state_broadcaster:frame_id`: The TF frame ID to be used in the published JointState messages. + ## Tools ### Schema Definition diff --git a/schema/schema.usda b/schema/schema.usda index 2641b6d..5978ace 100644 --- a/schema/schema.usda +++ b/schema/schema.usda @@ -1,7 +1,7 @@ #usda 1.0 ( - "ROS 2 Integration Schemas for OpenUSD Simulation Asset Interoperability." - " Defines declarative, engine-agnostic API schemas for ROS 2 interfaces" + "ROS Integration Schemas for OpenUSD Simulation Asset Interoperability." + " Defines declarative, engine-agnostic API schemas for ROS interfaces" " as specified in REP-XXXX, Section 2." subLayers = [ @usd/schema.usda@ @@ -10,9 +10,9 @@ over "GLOBAL" ( customData = { - string libraryName = "usdRos2" - string libraryPath = "usdRos2" - string libraryPrefix = "UsdRos2" + string libraryName = "usdRos" + string libraryPath = "usdRos" + string libraryPrefix = "UsdRos" dictionary libraryTokens = { # --- Role tokens (shared by Topic, Service, Action) --- dictionary publisher = { @@ -55,7 +55,7 @@ over "GLOBAL" ( # --- Frame tokens --- dictionary world = { - string doc = """Default parent frame for the top-most Ros2ContextAPI.""" + string doc = """Default parent frame for the top-most RosContextAPI.""" } } } @@ -64,10 +64,10 @@ over "GLOBAL" ( } # ====================================================================== -# Section 2.1 - Ros2ContextAPI +# Section 2.1 - RosContextAPI # ====================================================================== -class "Ros2ContextAPI" ( +class "RosContextAPI" ( inherits = customData = { token apiSchemaType = "singleApply" @@ -83,7 +83,7 @@ class "Ros2ContextAPI" ( """ ) { - uniform string ros2:context:namespace ( + uniform string ros:context:namespace ( customData = { string apiName = "namespace" } @@ -91,7 +91,7 @@ class "Ros2ContextAPI" ( The namespace is additive in the asset hierarchy.""" ) - uniform int ros2:context:domain_id ( + uniform int ros:context:domain_id ( customData = { string apiName = "domainId" } @@ -99,7 +99,7 @@ class "Ros2ContextAPI" ( descending from this context.""" ) - uniform string ros2:context:parent_frame = "world" ( + uniform string ros:context:parent_frame = "world" ( customData = { string apiName = "parentFrame" } @@ -111,10 +111,10 @@ class "Ros2ContextAPI" ( } # ====================================================================== -# Section 2.4 - Ros2TopicAPI +# Section 2.4 - RosTopicAPI # ====================================================================== -class "Ros2TopicAPI" ( +class "RosTopicAPI" ( inherits = customData = { token apiSchemaType = "singleApply" @@ -123,7 +123,7 @@ class "Ros2TopicAPI" ( doc = """Applied to prims exchanging streaming ROS 2 data. Simulators spawn the corresponding publisher or subscription at - runtime. Robot-wide interfaces go on or beneath the Ros2ContextAPI + runtime. Robot-wide interfaces go on or beneath the RosContextAPI prim; sensor interfaces go on a child Xform of the physical link, one interface per prim. Interface prims must reside outside payloads and do not generate TF frames. @@ -134,7 +134,7 @@ class "Ros2TopicAPI" ( # Core Attributes (Required) # ------------------------------------------------------------------ - uniform token ros2:topic:role ( + uniform token ros:topic:role ( customData = { string apiName = "role" } @@ -143,17 +143,17 @@ class "Ros2TopicAPI" ( subscription.""" ) - uniform string ros2:topic:name ( + uniform string ros:topic:name ( customData = { string apiName = "name" } doc = """The topic name, relative to the active ROS 2 namespace - inherited from the nearest ancestor Ros2ContextAPI. Must adhere + inherited from the nearest ancestor RosContextAPI. Must adhere to ROS 2 topic naming rules (alphanumeric, underscores, and forward slashes only; cannot start with a number).""" ) - uniform string ros2:topic:type ( + uniform string ros:topic:type ( customData = { string apiName = "type" } @@ -163,7 +163,7 @@ class "Ros2TopicAPI" ( must safely disable this interface and emit a warning.""" ) - uniform double ros2:topic:publish_rate ( + uniform double ros:topic:publish_rate ( customData = { string apiName = "publishRate" } @@ -175,7 +175,7 @@ class "Ros2TopicAPI" ( # Quality of Service (QoS) Attributes # ------------------------------------------------------------------ - uniform bool ros2:topic:qos:match_publisher = false ( + uniform bool ros:topic:qos:match_publisher = false ( customData = { string apiName = "qosMatchPublisher" } @@ -184,7 +184,7 @@ class "Ros2TopicAPI" ( publisher, ignoring explicit reliability and durability settings.""" ) - uniform token ros2:topic:qos:reliability = "system_default" ( + uniform token ros:topic:qos:reliability = "system_default" ( customData = { string apiName = "qosReliability" } @@ -194,7 +194,7 @@ class "Ros2TopicAPI" ( which maps to best-effort.""" ) - uniform token ros2:topic:qos:durability = "system_default" ( + uniform token ros:topic:qos:durability = "system_default" ( customData = { string apiName = "qosDurability" } @@ -203,7 +203,7 @@ class "Ros2TopicAPI" ( publishers should use 'transient_local'.""" ) - uniform token ros2:topic:qos:history = "system_default" ( + uniform token ros:topic:qos:history = "system_default" ( customData = { string apiName = "qosHistory" } @@ -211,7 +211,7 @@ class "Ros2TopicAPI" ( doc = """Maps to rmw_qos_profile_t history policy.""" ) - uniform int ros2:topic:qos:depth = 10 ( + uniform int ros:topic:qos:depth = 10 ( customData = { string apiName = "qosDepth" } @@ -220,10 +220,10 @@ class "Ros2TopicAPI" ( } # ====================================================================== -# Section 2.5 - Ros2ServiceAPI +# Section 2.5 - RosServiceAPI # ====================================================================== -class "Ros2ServiceAPI" ( +class "RosServiceAPI" ( inherits = customData = { token apiSchemaType = "singleApply" @@ -232,11 +232,11 @@ class "Ros2ServiceAPI" ( doc = """Applied to prims handling synchronous ROS 2 requests. Simulators spawn the corresponding server or client at runtime. - Same placement, resolution, and TF exclusion rules as Ros2TopicAPI. + Same placement, resolution, and TF exclusion rules as RosTopicAPI. """ ) { - uniform token ros2:service:role ( + uniform token ros:service:role ( customData = { string apiName = "role" } @@ -245,7 +245,7 @@ class "Ros2ServiceAPI" ( client. Simulation assets are typically servers.""" ) - uniform string ros2:service:name ( + uniform string ros:service:name ( customData = { string apiName = "name" } @@ -253,7 +253,7 @@ class "Ros2ServiceAPI" ( Must adhere to ROS 2 naming rules.""" ) - uniform string ros2:service:type ( + uniform string ros:service:type ( customData = { string apiName = "type" } @@ -263,10 +263,10 @@ class "Ros2ServiceAPI" ( } # ====================================================================== -# Section 2.6 - Ros2ActionAPI +# Section 2.6 - RosActionAPI # ====================================================================== -class "Ros2ActionAPI" ( +class "RosActionAPI" ( inherits = customData = { token apiSchemaType = "singleApply" @@ -275,11 +275,11 @@ class "Ros2ActionAPI" ( doc = """Applied to prims handling asynchronous, long-running ROS 2 behaviors. Simulators spawn the corresponding server or client at runtime. - Same placement, resolution, and TF exclusion rules as Ros2TopicAPI. + Same placement, resolution, and TF exclusion rules as RosTopicAPI. """ ) { - uniform token ros2:action:role ( + uniform token ros:action:role ( customData = { string apiName = "role" } @@ -288,7 +288,7 @@ class "Ros2ActionAPI" ( client. Simulation assets are typically servers.""" ) - uniform string ros2:action:name ( + uniform string ros:action:name ( customData = { string apiName = "name" } @@ -296,7 +296,7 @@ class "Ros2ActionAPI" ( Must adhere to ROS 2 naming rules.""" ) - uniform string ros2:action:type ( + uniform string ros:action:type ( customData = { string apiName = "type" } @@ -307,10 +307,10 @@ class "Ros2ActionAPI" ( } # ====================================================================== -# Section 2.7 - Ros2FrameAPI +# Section 2.7 - RosFrameAPI # ====================================================================== -class "Ros2FrameAPI" ( +class "RosFrameAPI" ( inherits = customData = { token apiSchemaType = "singleApply" @@ -325,7 +325,7 @@ class "Ros2FrameAPI" ( """ ) { - uniform string ros2:frame:id ( + uniform string ros:frame:id ( customData = { string apiName = "id" } @@ -333,7 +333,7 @@ class "Ros2FrameAPI" ( Prim name is used as the frame_id.""" ) - uniform bool ros2:frame:static = true ( + uniform bool ros:frame:static = true ( customData = { string apiName = "static" } @@ -342,3 +342,359 @@ class "Ros2FrameAPI" ( USD TimeSamples), the simulator broadcasts to /tf.""" ) } + +# ====================================================================== +# Section 4 - Robot Control +# +# Two approaches to simulating a robotic system are defined: +# +# Control-level simulation (Section 4.1): +# The simulator exposes joint state and command interfaces to an +# external control algorithm. The interface shape is simulator- +# specific (e.g. ros2_control hardware_interface, IPC, HIL). +# Prims are marked with RosExternalControlAPI as a built-in. +# +# Application-level (integrated) simulation (Section 4.2): +# The controller runs inside the simulator and is configured via +# prim parameters and ROS communication. Prims are marked with +# RosIntegratedControlAPI as a built-in and reference topic, +# service, or action interface prims. +# ====================================================================== + +# ====================================================================== +# Section 4.1 - RosExternalControlAPI +# ====================================================================== + +class "RosExternalControlAPI" ( + inherits = + customData = { + token apiSchemaType = "singleApply" + string className = "ExternalControlAPI" + } + doc = """Base schema for exposing robot control interfaces to external control algorithms. + + Applied (via 'prepend apiSchemas') by simulator-specific schemas to + identify prims whose joint state and command interfaces should be + exposed to an external control entity (e.g. a ros2_control + hardware_interface plugin, an inter-process controller, or a + hardware-in-the-loop connection). + """ +) +{ +} + +# ====================================================================== +# Section 4.2 - RosIntegratedControlAPI +# ====================================================================== + +class "RosIntegratedControlAPI" ( + inherits = + customData = { + token apiSchemaType = "singleApply" + string className = "IntegratedControlAPI" + } + doc = """Base schema for integrated (application-level) robot controllers. + + Included as a built-in via 'prepend apiSchemas' by simulator-specific + or standardized controller schemas that run the controller logic + inside the simulator. All prims referenced by a controller that + inherits this schema must have at least one of RosTopicAPI, + RosServiceAPI, or RosActionAPI applied. USD does not enforce this + constraint at the schema level; simulators are responsible for + runtime validation. + """ +) +{ +} + +# ====================================================================== +# Section 4.2.1.1 - RosRigidBodyTwistControllerAPI +# ====================================================================== + +class "RosRigidBodyTwistControllerAPI" ( + inherits = + prepend apiSchemas = ["RosIntegratedControlAPI"] + customData = { + token apiSchemaType = "singleApply" + string className = "RigidBodyTwistControllerAPI" + } + doc = """Controls a rigid body via geometry_msgs/Twist messages. + + Subscribes to a Twist topic and applies the commanded linear and + angular velocities directly to a rigid body in the physics engine, + with optional velocity and acceleration limits. Implementation should + follow logic similar to the diff_drive_controller in ros2_controllers. + """ +) +{ + rel ros:rigid_body_twist:subscriber ( + customData = { + string apiName = "subscriber" + } + doc = """Reference to a prim with RosTopicAPI configured as a + subscription for geometry_msgs/Twist control messages.""" + ) + + rel ros:rigid_body_twist:body ( + customData = { + string apiName = "body" + } + doc = """Reference to a prim with UsdPhysicsRigidBodyAPI to which + the commanded velocities are applied.""" + ) + + uniform double ros:rigid_body_twist:cmd_vel_timeout = 0.5 ( + customData = { + string apiName = "cmdVelTimeout" + } + doc = """Timeout in seconds after which a velocity command is + considered stale and the body velocity is zeroed.""" + ) + + uniform double ros:rigid_body_twist:linear:x:max_velocity ( + customData = { + string apiName = "linearXMaxVelocity" + } + doc = """Maximum linear velocity along the X axis in m/s.""" + ) + + uniform double ros:rigid_body_twist:linear:x:max_acceleration ( + customData = { + string apiName = "linearXMaxAcceleration" + } + doc = """Maximum linear acceleration along the X axis in m/s².""" + ) + + uniform double ros:rigid_body_twist:angular:z:max_velocity ( + customData = { + string apiName = "angularZMaxVelocity" + } + doc = """Maximum angular velocity around the Z axis in rad/s.""" + ) + + uniform double ros:rigid_body_twist:angular:z:max_acceleration ( + customData = { + string apiName = "angularZMaxAcceleration" + } + doc = """Maximum angular acceleration around the Z axis in rad/s².""" + ) +} + +# ====================================================================== +# Section 4.2.1.2 - RosDiffDriveControllerAPI +# ====================================================================== + +class "RosDiffDriveControllerAPI" ( + inherits = + prepend apiSchemas = ["RosIntegratedControlAPI"] + customData = { + token apiSchemaType = "singleApply" + string className = "DiffDriveControllerAPI" + } + doc = """Controls a differential drive robot via geometry_msgs/Twist messages. + + Subscribes to a Twist topic, converts commanded linear and angular + velocities into individual wheel velocities, and applies them to + wheel joint prims with UsdPhysicsDriveAPI. Publishes odometry data + as nav_msgs/Odometry. Implementation should follow logic similar to + the diff_drive_controller in ros2_controllers. + """ +) +{ + rel ros:diff_drive:subscriber ( + customData = { + string apiName = "subscriber" + } + doc = """Reference to a prim with RosTopicAPI configured as a + subscription for geometry_msgs/Twist velocity commands.""" + ) + + rel ros:diff_drive:odom ( + customData = { + string apiName = "odom" + } + doc = """Reference to a prim with RosTopicAPI configured as a + publisher for nav_msgs/Odometry data.""" + ) + + rel ros:diff_drive:left_wheels ( + customData = { + string apiName = "leftWheels" + } + doc = """References to prims with UsdPhysicsDriveAPI representing + the left wheel joints.""" + ) + + rel ros:diff_drive:right_wheels ( + customData = { + string apiName = "rightWheels" + } + doc = """References to prims with UsdPhysicsDriveAPI representing + the right wheel joints.""" + ) + + uniform double ros:diff_drive:wheel_separation ( + customData = { + string apiName = "wheelSeparation" + } + doc = """Distance between the left and right wheel contact points + in meters.""" + ) + + uniform double ros:diff_drive:wheel_radius ( + customData = { + string apiName = "wheelRadius" + } + doc = """Radius of the wheels in meters.""" + ) + + uniform double ros:diff_drive:cmd_vel_timeout = 0.5 ( + customData = { + string apiName = "cmdVelTimeout" + } + doc = """Timeout in seconds after which a velocity command is + considered stale and wheel velocities are zeroed.""" + ) + + uniform double ros:diff_drive:linear:x:max_velocity ( + customData = { + string apiName = "linearXMaxVelocity" + } + doc = """Maximum linear velocity along the X axis in m/s.""" + ) + + uniform double ros:diff_drive:linear:x:max_acceleration ( + customData = { + string apiName = "linearXMaxAcceleration" + } + doc = """Maximum linear acceleration along the X axis in m/s².""" + ) + + uniform double ros:diff_drive:angular:z:max_velocity ( + customData = { + string apiName = "angularZMaxVelocity" + } + doc = """Maximum angular velocity around the Z axis in rad/s.""" + ) + + uniform double ros:diff_drive:angular:z:max_acceleration ( + customData = { + string apiName = "angularZMaxAcceleration" + } + doc = """Maximum angular acceleration around the Z axis in rad/s².""" + ) +} + +# ====================================================================== +# Section 4.2.1.3 - RosJointTrajectoryControllerAPI +# ====================================================================== + +class "RosJointTrajectoryControllerAPI" ( + inherits = + prepend apiSchemas = ["RosIntegratedControlAPI"] + customData = { + token apiSchemaType = "singleApply" + string className = "JointTrajectoryControllerAPI" + } + doc = """Executes joint trajectories via control_msgs/FollowJointTrajectory actions. + + Accepts action goals and commands the simulator to follow the + specified joint trajectory. Implementation should follow logic + similar to the joint_trajectory_controller in ros2_controllers. + """ +) +{ + rel ros:joint_trajectory:server ( + customData = { + string apiName = "server" + } + doc = """Reference to a prim with RosActionAPI configured as a + server for control_msgs/FollowJointTrajectory action goals.""" + ) + + rel ros:joint_trajectory:command_joints ( + customData = { + string apiName = "commandJoints" + } + doc = """References to prims with UsdPhysicsJointAPI representing + the joints to be commanded along the trajectory. Simulators must + use the ros:joint:name custom property for joint name mapping, + falling back to the prim name if absent.""" + ) + + uniform double ros:joint_trajectory:action_monitor_rate ( + customData = { + string apiName = "actionMonitorRate" + } + doc = """Frequency in Hz at which trajectory execution progress + is monitored.""" + ) + + uniform double ros:joint_trajectory:stopped_velocity_tolerance ( + customData = { + string apiName = "stoppedVelocityTolerance" + } + doc = """Velocity tolerance at the end of the trajectory below + which the controlled system is considered stopped.""" + ) + + uniform double ros:joint_trajectory:timeout ( + customData = { + string apiName = "timeout" + } + doc = """Maximum time in seconds allowed to reach the trajectory + goal before the action is aborted.""" + ) +} + +# ====================================================================== +# Section 4.2.1.4 - RosJointStateBroadcasterAPI +# ====================================================================== + +class "RosJointStateBroadcasterAPI" ( + inherits = + prepend apiSchemas = ["RosIntegratedControlAPI"] + customData = { + token apiSchemaType = "singleApply" + string className = "JointStateBroadcasterAPI" + } + doc = """Reads joint states and publishes them as sensor_msgs/JointState messages. + + Implementation should follow logic similar to the + joint_state_broadcaster in ros2_controllers. + """ +) +{ + rel ros:joint_state_broadcaster:publisher ( + customData = { + string apiName = "publisher" + } + doc = """Reference to a prim with RosTopicAPI configured as a + publisher for sensor_msgs/JointState messages.""" + ) + + rel ros:joint_state_broadcaster:joints ( + customData = { + string apiName = "joints" + } + doc = """References to prims with UsdPhysicsJointAPI representing + the joints whose states are to be broadcast. Simulators must use + the ros:joint:name custom property for joint name mapping, + falling back to the prim name if absent.""" + ) + + uniform double ros:joint_state_broadcaster:publish_rate ( + customData = { + string apiName = "publishRate" + } + doc = """Frequency in Hz at which joint states are published.""" + ) + + uniform string ros:joint_state_broadcaster:frame_id ( + customData = { + string apiName = "frameId" + } + doc = """The TF frame_id to be used in the published JointState + messages.""" + ) +}