Add simulation clock publisher

README.md

@@ -27,7 +27,9 @@ MuJoCo Plugin for integrating with ROS 2
   - [Loading the Plugin in MJCF](#loading-the-plugin-in-mjcf)
   - [Plugin Configuration](#plugin-configuration)
 - [🔄 ROS 2 Interface](#-ros-2-interface)
-  - [⬅️ Published Topics (Sensors)](#️-published-topics-sensors)
+  - [⬅️ Published Topics](#️-published-topics)
+    - [Sensor Data](#sensor-data)
+    - [Simulation Clock](#simulation-clock)
   - [➡️ Subscribed Topics (Actuators)](#️-subscribed-topics-actuators)
 - [💡 Example](#-example)
 - [👥 Contributing](#-contributing)
@@ -53,17 +55,20 @@ Mujoco ROS 2 topics plotted on [PlotJuggler](https://github.com/facontidavide/Pl
 
 ## 🚀 Features
 
-- **ROS 2 Node Integration**: Automatically initializes and manages a dedicated ROS 2 node (`mujoco_ros2_plugin`).
+- **ROS 2 Node Integration**: Automatically initializes and manages a dedicated ROS 2 node.
 - **Sensor Data Publication**:
   - Publishes data from all MuJoCo sensors defined in your model.
   - Supports single-value sensors (`example_interfaces::msg::Float64`).
   - Supports multi-dimensional sensors (`example_interfaces::msg::Float64MultiArray`).
+- **Simulation Clock Publication**:
+  - Publishes MuJoCo simulation time as `builtin_interfaces::msg::Time` messages.
+  - Enables time synchronization between MuJoCo simulation and ROS 2 nodes.
 - **Actuator Command Subscription**:
   - Subscribes to ROS 2 topics for controlling MuJoCo actuators.
   - Accepts `example_interfaces::msg::Float64` for actuator inputs.
 - **Configurable**:
   - Set a custom ROS 2 namespace for all topics (default: `"mujoco/"`).
-  - Adjust the queue size for ROS 2 publishers and subscribers (default: `1`).
+  - Adjust the reliability param of the QoS for the actuators subscribers (default: `best_effort`).
 - **Passive MuJoCo Plugin**: Operates within MuJoCo's simulation loop for timely data exchange.
 - **Easy to Build & Install**: Standard CMake build process.
 
@@ -74,7 +79,7 @@ Ensure the following are installed and configured on your system:
 - **MuJoCo Physics Simulator**:
   - The build system tries to find MuJoCo via the `simulate` program in your `PATH`.
   - Alternatively, set the `MUJOCO_PATH` environment variable to your MuJoCo `bin` directory (e.g., `~/.mujoco/mujoco-X.Y.Z/bin`).
-- **ROS 2**: A C++20 compatible ROS 2 distribution (e.g., Humble, Iron).
+- **ROS 2**: A C++20 compatible ROS 2 distribution (e.g., Jazzy, Humble).
   - Source your ROS 2 environment: `source /opt/ros/<your_ros_distro>/setup.bash`
   - Required ROS 2 packages: `rclcpp`, `example_interfaces`, `ament_cmake`.
 - **Build Tools**:
@@ -112,7 +117,7 @@ Ensure the following are installed and configured on your system:
 MuJoCo needs to register the plugin for every simulation, the installation step usually places it in a standard location so that it can be automatically registered. If you install it elsewhere, or MuJoCo cannot find it, you might need to:
 
 - **Option 1:** Move the library output file manually to the `mujoco_plugin` folder located at the same directory as the mujoco executable.
-- **Option 2:**: Register the plugin manually by calling the `Ros2Plugin::register_plugin()` function before the mujoco simulate loop.
+- **Option 2:** Register the plugin manually by calling the `Ros2Plugin::register_plugin()` function before the mujoco simulate loop.
 
 ## ▶️ Usage
 
@@ -122,18 +127,17 @@ To activate the plugin, add an `<extension>` block to your MJCF model file. The
 
 ```xml
 <mujoco>
-    <extension>
-        <plugin plugin="mujoco.ros2">
-            <instance name="ros2_plugin">
-                <!-- Optional: Configuration parameters -->
-                <!-- <config key="ros_namespace" value="mujoco/"/> -->
-                <!-- <config key="topic_queue_size" value="1"/> -->
-                <!-- <config key="topic_reliability" value="reliable"/> -->
-            </instance>
-        </plugin>
-    </extension>
-
-    <!-- ... rest of your MJCF model ... -->
+  <extension>
+    <plugin plugin="mujoco.ros2">
+      <instance name="ros2_plugin">
+        <!-- Optional: Configuration parameters -->
+        <!-- <config key="ros_namespace" value="mujoco/"/> -->
+        <!-- <config key="subscribers_reliability" value="best_effort"/> -->
+      </instance>
+    </plugin>
+  </extension>
+
+  <!-- ... rest of your MJCF model ... -->
 </mujoco>
 ```
 
@@ -148,37 +152,41 @@ Configure the plugin directly within the MJCF `<plugin>` tag:
   <config key="ros_namespace" value="custom_namespace/"/>
   ```
 
-- **`topic_queue_size`** (integer, default: `1`):
-  Defines the queue size for ROS 2 publishers and subscribers.
-
-  ```xml
-  <config key="topic_queue_size" value="5"/>
-  ```
-
-- **`topic_reliability`** (string, default: `reliable`):
-  Sets the reliability of the ROS topic QoS.
+- **`subscribers_reliability`** (string, default: `best_effort`):
+  Sets the reliability of the ROS subscribers QoS.
 
   ```xml
-  <config key="topic_reliability" value="best_effort"/>
+  <config key="subscribers_reliability" value="reliable"/>
   ```
 
 ## 🔄 ROS 2 Interface
 
 The plugin establishes the following ROS 2 communication channels:
 
-### ⬅️ Published Topics (Sensors)
+### ⬅️ Published Topics
+
+#### Sensor Data
 
 Data from sensors defined in the `<sensor>` section of your MJCF will be published.
 
-- **Single-Dimension Sensors**:
+- **Scalar Sensors**:
   - Topic: `<ros_namespace>sensors/<sensor_name>`
   - Type: `example_interfaces::msg::Float64`
-- **Multi-Dimension Sensors**:
+- **Array Sensors**:
   - Topic: `<ros_namespace>sensors/<sensor_name>`
   - Type: `example_interfaces::msg::Float64MultiArray`
 
 Where `<sensor_name>` is the `name` attribute from the MJCF.
 
+#### Simulation Clock
+
+The plugin publishes the MuJoCo simulation time for synchronization with ROS 2 nodes.
+
+- **Clock Topic**:
+  - Topic: `<ros_namespace>clock`
+  - Type: `builtin_interfaces::msg::Time`
+  - Contains the current MuJoCo simulation time in seconds and nanoseconds.
+
 ### ➡️ Subscribed Topics (Actuators)
 
 Commands for actuators defined in the `<actuator>` section of your MJCF can be sent via these topics.
@@ -196,12 +204,12 @@ Consider this MJCF snippet for a simple car:
 ```xml
 <mujoco>
   <extension>
-      <plugin plugin="mujoco.ros2">
-        <instance name="ros2_plugin">
-          <config key="ros_namespace" value="simple_car/"/>
-        </instance>
-      </plugin>
-    </extension>
+    <plugin plugin="mujoco.ros2">
+    <instance name="ros2_plugin">
+      <config key="ros_namespace" value="simple_car/"/>
+    </instance>
+    </plugin>
+  </extension>
 
   <asset>
     <mesh name="chasis" scale=".01 .006 .0015"
@@ -257,6 +265,7 @@ Consider this MJCF snippet for a simple car:
 **This would result in:**
 
 - **Publishers**:
+  - `/simple_car/clock` (`Time`)
   - `/simple_car/sensors/left_torque` (`Float64`)
   - `/simple_car/sensors/right_torque` (`Float64`)
 - **Subscribers**:
@@ -266,6 +275,9 @@ Consider this MJCF snippet for a simple car:
 Interact using ROS 2 tools:
 
 ```bash
+# Listen to simulation clock
+ros2 topic echo /simple_car/clock
+
 # Listen to a sensor
 ros2 topic echo /simple_car/sensors/left_torque
 

src/register.cpp

@@ -6,5 +6,4 @@ namespace mujoco::plugin::ros2 {
 mjPLUGIN_LIB_INIT {
     Ros2Plugin::register_plugin();
 }
-
 }  // namespace mujoco::plugin::ros2

src/ros2_plugin.cpp

@@ -1,11 +1,13 @@
-#include <algorithm>
 #include <bit>
+#include <cmath>
 #include <mujoco/mjplugin.h>
 #include <mujoco/mujoco.h>
 
 #include "ros2_plugin.hpp"
 
 namespace mujoco::plugin::ros2 {
+static constexpr unsigned int sec_to_nsec = 1e9;
+
 int read_int_attr(const char* value, int default_value) {
     if (value == nullptr || value[0] == '\0') {
         return default_value;
@@ -29,22 +31,20 @@ Ros2Plugin::Config Ros2Plugin::get_config_from_model(const mjModel* model, int i
     config.ros_namespace =
         read_string_attr(mj_getPluginConfig(model, instance, attr_key_ros_namespace), attr_default_ros_namespace);
 
-    config.topic_queue_size =
-        read_int_attr(mj_getPluginConfig(model, instance, attr_key_topic_queue_size), attr_default_topic_queue_size);
-
-    config.topic_reliability = read_string_attr(
-        mj_getPluginConfig(model, instance, attr_key_topic_reliability), attr_default_topic_reliability
+    config.subscribers_reliability = read_string_attr(
+        mj_getPluginConfig(model, instance, attr_key_subscribers_reliability), attr_default_subscribers_reliability
     );
 
     return config;
 }
 
 Ros2Plugin::Ros2Plugin(const Config& config) :
-    Node{config.node_name}, ros_namespace{config.ros_namespace}, qos{rclcpp::KeepLast(config.topic_queue_size)} {
-    if (config.topic_reliability == "best_effort") {
-        this->qos = this->qos.best_effort();
-    } else {
-        this->qos = this->qos.reliable();
+    Node{config.node_name},
+    ros_namespace{config.ros_namespace},
+    subscribers_qos{rclcpp::KeepLast(1)},
+    publishers_qos{rclcpp::KeepLast(1)} {
+    if (config.subscribers_reliability != "reliable") {
+        this->subscribers_qos = this->subscribers_qos.best_effort();
     }
 
     RCLCPP_INFO(this->get_logger(), "ROS2 Mujoco Plugin node started");
@@ -53,47 +53,49 @@ Ros2Plugin::Ros2Plugin(const Config& config) :
 void Ros2Plugin::create_sensor_publishers(const mjModel* model) {
     RCLCPP_INFO(this->get_logger(), "Creating sensor publishers for %d sensors", model->nsensor);
 
+    this->clock_msg.sec = 0;
+    this->clock_msg.nanosec = 0;
+    this->clock_publisher =
+        this->create_publisher<builtin_interfaces::msg::Time>(this->ros_namespace + "clock", this->publishers_qos);
+
+    RCLCPP_INFO(this->get_logger(), "Created clock publisher on topic '%sclock'", this->ros_namespace.c_str());
+
     for (int i = 0; i < model->nsensor; i++) {
         const char*       sensor_name = model->names + model->name_sensoradr[i];
         const std::string topic_name = this->ros_namespace + "sensors/" + std::string(sensor_name);
-
-        int  pub_index = 0;
-        int  num_dimensions = model->sensor_dim[i];
-        auto sensor_datatype = static_cast<mjtDataType>(model->sensor_datatype[i]);
+        const int         num_dimensions = model->sensor_dim[i];
 
         if (num_dimensions == 1) {
-            auto pub = this->create_publisher<example_interfaces::msg::Float64>(topic_name, this->qos);
-            double_sensor_publishers.push_back(pub);
-            pub_index = static_cast<int>(double_sensor_publishers.size() - 1);
+            auto pub = this->create_publisher<example_interfaces::msg::Float64>(topic_name, this->publishers_qos);
+            this->scalar_sensor_publishers.push_back(pub);
+            this->subscribers_indexes.push_back(this->scalar_sensor_publishers.size() - 1);
         } else {
-            auto pub = this->create_publisher<example_interfaces::msg::Float64MultiArray>(topic_name, this->qos);
-            multiarray_sensor_publishers.push_back(pub);
-            pub_index = static_cast<int>(multiarray_sensor_publishers.size() - 1);
+            auto pub =
+                this->create_publisher<example_interfaces::msg::Float64MultiArray>(topic_name, this->publishers_qos);
+            this->array_sensor_publishers.push_back(pub);
+            this->subscribers_indexes.push_back(this->array_sensor_publishers.size() - 1);
         }
 
         RCLCPP_INFO(
             this->get_logger(), "Created publisher for sensor '%s' on topic '%s'", sensor_name, topic_name.c_str()
         );
-
-        this->sensors.push_back({i, pub_index});
     }
 }
 
-void Ros2Plugin::create_actuator_subscribers(const mjModel* model) {
+void Ros2Plugin::create_actuator_subscribers(const mjModel* model, mjData* data) {
     RCLCPP_INFO(this->get_logger(), "Creating actuator subscribers for %d actuators", model->nu);
 
     for (int i = 0; i < model->nu; i++) {
         const char* actuator_name = model->names + model->name_actuatoradr[i];
         std::string topic_name = this->ros_namespace + "actuators/" + actuator_name + "/command";
 
         auto sub = this->create_subscription<example_interfaces::msg::Float64>(
+            topic_name, this->subscribers_qos,
             // NOLINTNEXTLINE(performance-unnecessary-value-param)
-            topic_name, this->qos, [](const example_interfaces::msg::Float64::SharedPtr /*msg*/) {}
+            [data, i](const example_interfaces::msg::Float64::SharedPtr msg) { data->ctrl[i] = msg->data; }
         );
 
         actuator_subscribers.push_back(sub);
-        this->actuators.push_back({i, static_cast<int>(actuator_subscribers.size() - 1)});
-
         RCLCPP_INFO(
             this->get_logger(), "Created subscriber for actuator '%s' on topic '%s'", actuator_name, topic_name.c_str()
         );
@@ -103,47 +105,47 @@ void Ros2Plugin::create_actuator_subscribers(const mjModel* model) {
 void Ros2Plugin::compute(const mjModel* model, mjData* data) {
     if (not initialized) {
         this->create_sensor_publishers(model);
-        this->create_actuator_subscribers(model);
+        this->create_actuator_subscribers(model, data);
         initialized = true;
     }
 
-    for (const auto& sensor : this->sensors) {
-        int     num_dimensions = model->sensor_dim[sensor.model_index];
-        int     sensor_address = model->sensor_adr[sensor.model_index];
-        mjtNum* sensor_data = &data->sensordata[sensor_address];
+    rclcpp::spin_some(this->get_node_base_interface());
+
+    this->clock_msg.nanosec += std::lround(model->opt.timestep * sec_to_nsec);
+
+    if (this->clock_msg.nanosec >= sec_to_nsec) {
+        this->clock_msg.sec += static_cast<int32_t>(this->clock_msg.nanosec / sec_to_nsec);
+        this->clock_msg.nanosec %= sec_to_nsec;
+    }
+
+    this->clock_publisher->publish(this->clock_msg);
+
+    for (int i = 0; i < model->nsensor; i++) {
+        const int num_dimensions = model->sensor_dim[i];
+        const int sensor_address = model->sensor_adr[i];
+        mjtNum*   sensor_data = &data->sensordata[sensor_address];
 
         if (num_dimensions == 1) {
             example_interfaces::msg::Float64 msg;
             msg.data = sensor_data[0];
-            this->double_sensor_publishers[sensor.comm_index]->publish(msg);
+            this->scalar_sensor_publishers[this->subscribers_indexes[i]]->publish(msg);
         } else {
             example_interfaces::msg::Float64MultiArray msg;
             msg.layout.dim.resize(1);
             msg.layout.dim[0].size = num_dimensions;
             msg.layout.dim[0].stride = num_dimensions;
-            msg.data.resize(num_dimensions);
-            std::copy(sensor_data, sensor_data + num_dimensions, msg.data.begin());
-            this->multiarray_sensor_publishers[sensor.comm_index]->publish(msg);
-        }
-    }
-
-    for (auto& actuator : this->actuators) {
-        example_interfaces::msg::Float64 msg;
-        rclcpp::MessageInfo              info;
-
-        if (this->actuator_subscribers[actuator.comm_index]->take(msg, info)) {
-            data->ctrl[actuator.model_index] = msg.data;
+            msg.data.assign(sensor_data, sensor_data + num_dimensions);
+            this->array_sensor_publishers[this->subscribers_indexes[i]]->publish(msg);
         }
     }
 }
 
 void Ros2Plugin::reset() {
     RCLCPP_INFO(this->get_logger(), "Resetting ROS2 Plugin state");
     this->initialized = false;
-    this->sensors.clear();
-    this->actuators.clear();
-    this->double_sensor_publishers.clear();
-    this->multiarray_sensor_publishers.clear();
+    this->subscribers_indexes.clear();
+    this->scalar_sensor_publishers.clear();
+    this->array_sensor_publishers.clear();
     this->actuator_subscribers.clear();
 }
 
@@ -155,8 +157,7 @@ void Ros2Plugin::register_plugin() {
 
     std::vector<const char*> attributes = {
         attr_key_ros_namespace,
-        attr_key_topic_queue_size,
-        attr_key_topic_reliability,
+        attr_key_subscribers_reliability,
     };
 
     plugin.nattribute = static_cast<int>(attributes.size());