Inconsistent publishing rate for sensors

[alonsollorente]

Hi everyone!

I hope you can help me with this problem that I'm having. I'm trying to broadcast sensory data in ROS2 and going through some difficulties.

Thank you in advance for helping me out! I let you with a brief description of my problem.

Description of the bug
There is an inconsistency in the publishing frequency of sensor data between ROS2 and Unity when using ros_tcp_endpoint. Despite setting the publishing rate to 10Hz for one LiDAR sensor and 100Hz for an IMU, the actual publishing rate observed is significantly lower and inconsistent, being in the case of the IMU around 35Hz. The ros2 topic hz /imu output can be seen:

To Reproduce
Steps to reproduce the behavior:

• I'm using the set of sensors from UnitySensor, which uses ROS_TCP_Connector.
• Setting from IMU and LiDAR:

Environment set up:

• Unity Version: 2023.2.20f1
• Ubuntu 22.04, ROS2 Humble

[RyodoTanaka]

@alonsollorente
Thank you for updating the issue.
This issue is related to #156 (It is written in Japanese, but discussing same topic)
The main reason of this problem comes from using Update() function.
To fix this, we should use FixUpdate() function.
https://github.com/Field-Robotics-Japan/UnitySensors/blob/master/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/RosMsgPublisher.cs#L39
https://github.com/Field-Robotics-Japan/UnitySensors/blob/master/Assets/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs#L51

But, this problem is already fixed in develop branch.
Thus, please try to use develop branch.
Now, merging PR from develop into master is under development (#163)

[alonsollorente]

Hi!

Thank you for the fast answer. I checked that the Update() function in the LiDAR publisher case is the same in the develop and master branch. I don't know if maybe I'm approaching it wrong...

Furthermore, I tried the project in the develop branch, and is still inconsistent in frequency for both sensors.

[RyodoTanaka]

@alonsollorente

the Update() function in the LiDAR publisher case is the same in the develop.

Yes, you are right. It is still not fixed in develop branch about LiDAR Publisher.

I tried the project in the develop branch, and is still inconsistent in frequency for both sensors.

About this, I need to find the reason why especially for IMU sensors.
I think it takes a bit long time to fix. But if someone find the solution, comment it please.

[Bob-Eric]

Hi there!
I have tried these sensors in both ROS1 and ROS2 and they work fine.
However, you have to be aware of the frame rate in Unity.
Currently it is not possible to publish sensors faster than the frame rate.

[RyodoTanaka]

This Issue would related with following.
https://github.com/Field-Robotics-Japan/UnitySensors/pull/188/files#diff-5d75f813dd50b7d273b40a2fe62b8e907eb1bf7e23aac2d6fd35685f24272d34R54

[wangguohao-github]

Hi, i have fixed this problem. You could replace unitysensor.cs by the following.

`using UnityEngine;
// using Unity.Robotics.ROSTCPConnector;

namespace UnitySensors.Sensor
{
public abstract class UnitySensor : MonoBehaviour
{
[SerializeField]
private float _frequency = 10.0f;

    private float _time;
    private float _dt;
    private float _frequency_inv;
    private float _dt_step;

    public delegate void OnSensorUpdated();
    public OnSensorUpdated onSensorUpdated;

    public float sensorDeltaTime => _dt_step;


    public float time => _time;

    private void Awake()
    {
        _dt = 0.0f;
        _frequency_inv = 1.0f / _frequency;
        Init();
    }

    protected virtual void Update()
    {
        _dt += Time.deltaTime;

        while (_dt >= _frequency_inv)
        {
            _dt -= _frequency_inv;

            _time = Time.time;
            _dt_step = _frequency_inv;

            UpdateSensor();
        }
    }

    private void OnDestroy()
    {
        onSensorUpdated = null;
        OnSensorDestroy();
    }

    protected abstract void Init();
    protected abstract void UpdateSensor();
    protected abstract void OnSensorDestroy();
}

}
`