Not getting desire frequency

Unity Engine
, ,
1

I have written a c# code to publish robot odometry information but problem is that its not able to reach desire hz.

using System;
using System.Collections;
using System.Collections.Generic;
using Unity.Burst;
using Unity.Collections;
using UnityEngine;
using UnityEngine.Jobs;
using Unity.Jobs;
using Random = Unity.Mathematics.Random;

using Unity.Robotics.ROSTCPConnector;
using Unity.Robotics.ROSTCPConnector.ROSGeometry;
using RosMessageTypes.Nav;
using RosMessageTypes.Geometry;


public class OdomertryPublisher : MonoBehaviour
{
    ROSConnection ros;
    private OdometryMsg _message;

    public string topicName = "odom";
    public string frameId = "odom";

    public GameObject base_link;

    ArticulationBody rb;
    QuaternionMsg ang_pos;
    PointMsg lin_pos;
    Vector3<FLU> lin_vel, rot_vel;


    public float publishMessageFrequency = 100f;

    private float _timeElapsed;
    private float _timeStamp;

    // Start is called before the first frame update
    void Start()
    {

        ros = ROSConnection.GetOrCreateInstance();
        ros.RegisterPublisher<OdometryMsg>(topicName);

        rb = GetComponent<ArticulationBody>();
      
    }

    // Update is called once per frame
    void Update()
    {
        this._timeElapsed += Time.deltaTime;

      
      
        //Debug.Log(rot_vel[1]);

        this._message = new OdometryMsg();

        if (_timeElapsed > ( 1f / publishMessageFrequency))
        {
            // coverting from unity to ros axis
            ang_pos = base_link.transform.rotation.To<FLU>();
            lin_pos = base_link.transform.position.To<FLU>();
            rot_vel = rb.angularVelocity.To<FLU>();
            lin_vel = rb.velocity.To<FLU>();
          
            //Debug.Log(rot_vel);


            uint sec = (uint)Math.Truncate(this._timeStamp);
            uint nanosec = (uint)( (this._timeStamp - sec)*1e+9 );
            // Debug.Log(sec);
            this._message.header.stamp.sec = sec;
            this._message.header.stamp.nanosec = nanosec;
            this._message.header.frame_id = frameId;

            this._message.child_frame_id = base_link.transform.name;

            this._message.pose.pose.position.x = lin_pos.x;
            this._message.pose.pose.position.y = lin_pos.y;
            this._message.pose.pose.position.z = lin_pos.z;

            this._message.pose.pose.orientation.x = ang_pos.x;
            this._message.pose.pose.orientation.y = ang_pos.y;
            this._message.pose.pose.orientation.z = ang_pos.z;
            this._message.pose.pose.orientation.w = ang_pos.w;

            this._message.twist.twist.linear.x = lin_vel.x;
            this._message.twist.twist.linear.y = lin_vel.y;
            this._message.twist.twist.linear.z = lin_vel.z;

            this._message.twist.twist.angular.x = rot_vel.x;
            this._message.twist.twist.angular.y = rot_vel.y;
            this._message.twist.twist.angular.z = rot_vel.z;

            ros.Publish(this.topicName, this._message);

            this._timeElapsed = 0;
            this._timeStamp = Time.time;
        }
    }
}

I made it to publish 100hz
7848741--995469--upload_2022-1-28_19-26-29.png

But hardly its reaching out to 16 hz:
7848741--995472--upload_2022-1-28_19-28-35.png

I made a script similar to ros unity publisher but I do not understand why is it not reaching the required hz.
Even I have written bunch of other code they all have this problem. I feel there is some problem in the way its check for the time than publish it. Let me know how to solve this frequency problem. If possible do check and modify my code so that I can get required hz.

2

Hi @vermahrithik10,

your ros.Publish() statement is in the Update function. So your odometry frequency cannot be higher than the frequency of Update function itself. Your Update function is running as fast as your game (it is the frequency of your game). In the game window of the Unity editor you can click on Stats to see the frequency.

To solve your problem

  • you could move the ros.Publish() statement in an external thread and send the data more often, but your gameObjects will then have the same position in multiple messages.
  • you could try to optimize the performance of your game, but for 1000fps you need good hardware.
  • you could work not in real time (look at time scale).