Sensor's lastUpdateTime not updated when running lockstep mode

eschembor-irobot commented 1 year ago

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

Instantiate an IMU sensor
Run gazebo without lockstep mode, observe that timestamps are updated correctly
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 commented 1 year ago

You're absolutely right. The problem is that PR #2863 was never ported to Gazebo 11. I requested the forward port in https://github.com/gazebosim/gazebo-classic/pull/2863#issuecomment-1759786850 .

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 commented 1 year ago

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.

gazebosim / gazebo-classic