Sensor's lastUpdateTime not updated when running lockstep mode

[eschembor-irobot]

Environment

• OS Version: Ubuntu 20.04
• Source or binary build?
  gazebo v11.12.0

Description

• I would expect sensor data to be published with correct timestamps when running in lockstep mode
• Sensor data published with zeroed timestamps when running in lockstep mode

Steps to reproduce

1. Instantiate an IMU sensor
2. Run gazebo without lockstep mode, observe that timestamps are updated correctly
3. Run gazebo with lockstep mode, observe that timestamps are zeroed

Output

The gazebo11 branch appears to be missing a line that I see in the gazebo9 branch:
https://github.com/gazebosim/gazebo-classic/blob/gazebo9/gazebo/sensors/Sensor.cc#L185
https://github.com/gazebosim/gazebo-classic/blob/gazebo11/gazebo/sensors/Sensor.cc#L242

This line seems necessary to update the lastUpdateTime when running in lockstep mode that many of the sensor plugins use to timestamp their data.

[peci1]

You're absolutely right. The problem is that PR #2863 was never ported to Gazebo 11. I requested the forward port in #2863 (comment) .

This problem should only affect these plugins:

• gazebo_ros_imu_sensor
• gazebo_ros_block_laser
• gazebo_ros_range

Other sensors use LastMeasurementTime() which is probably handled correctly.

[peci1]

This is a workaround sensor plugin you can attach to the affected sensors that will update the lastUpdateTime variable. Use this until a proper fix is merged. Be aware that this workaround doesn't lock the mutex that protects lastUpdateTime. However, the Gazebo 9 PR that fixed this bug also did not lock the mutex, so hopefully it's not a big issue.

#include <sstream>
#define protected public
#include <gazebo/sensors/Sensor.hh>
#undef protected

#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.hh>

namespace gazebo
{

/**
 * \brief Workaround plugin sensor for issue https://github.com/gazebosim/gazebo-classic/issues/3336 in Gazebo 11.
 *
 * Just attach it to a sensor which forgets to update lastUpdateTime in lockstep mode and the time will get updated.
 */
class SensorUpdateTimeWorkaround : public SensorPlugin
{
public:
    void Load(sensors::SensorPtr sensor_, sdf::ElementPtr sdf_) override
    {
        this->sensor = sensor_;
        if (this->sensor == nullptr)
        {
            gzerr << "Error: SensorUpdateTimeWorkaround must be attached to a sensor!" << std::endl;
            return;
        }

        if (this->sensor->StrictRate())
        {
            this->updateConnection = this->sensor->ConnectUpdated([this]() {
                const auto world = gazebo::physics::get_world(this->sensor->WorldName());
                this->sensor->lastUpdateTime = world->SimTime();
            });
        }
    }

    gazebo::event::ConnectionPtr updateConnection;
    sensors::SensorPtr sensor {nullptr};  //!< The sensor of this plugin.
};

GZ_REGISTER_SENSOR_PLUGIN(SensorUpdateTimeWorkaround)
}

License of the code block: BSD-3-Clause. Copyright: Czech Technical University in Prague.