Search Unity

  1. Unity Asset Manager is now available in public beta. Try it out now and join the conversation here in the forums.
    Dismiss Notice

Question Not getting desire frequency

Discussion in 'Robotics' started by vermahrithik10, Jan 28, 2022.

  1. vermahrithik10

    vermahrithik10

    Joined:
    Jan 10, 2022
    Posts:
    12
    I have written a c# code to publish robot odometry information but problem is that its not able to reach desire hz.
    Code (CSharp):
    1. using System;
    2. using System.Collections;
    3. using System.Collections.Generic;
    4. using Unity.Burst;
    5. using Unity.Collections;
    6. using UnityEngine;
    7. using UnityEngine.Jobs;
    8. using Unity.Jobs;
    9. using Random = Unity.Mathematics.Random;
    10.  
    11. using Unity.Robotics.ROSTCPConnector;
    12. using Unity.Robotics.ROSTCPConnector.ROSGeometry;
    13. using RosMessageTypes.Nav;
    14. using RosMessageTypes.Geometry;
    15.  
    16.  
    17. public class OdomertryPublisher : MonoBehaviour
    18. {
    19.     ROSConnection ros;
    20.     private OdometryMsg _message;
    21.  
    22.     public string topicName = "odom";
    23.     public string frameId = "odom";
    24.  
    25.     public GameObject base_link;
    26.  
    27.     ArticulationBody rb;
    28.     QuaternionMsg ang_pos;
    29.     PointMsg lin_pos;
    30.     Vector3<FLU> lin_vel, rot_vel;
    31.  
    32.  
    33.     public float publishMessageFrequency = 100f;
    34.  
    35.     private float _timeElapsed;
    36.     private float _timeStamp;
    37.  
    38.     // Start is called before the first frame update
    39.     void Start()
    40.     {
    41.  
    42.         ros = ROSConnection.GetOrCreateInstance();
    43.         ros.RegisterPublisher<OdometryMsg>(topicName);
    44.  
    45.         rb = GetComponent<ArticulationBody>();
    46.      
    47.     }
    48.  
    49.     // Update is called once per frame
    50.     void Update()
    51.     {
    52.         this._timeElapsed += Time.deltaTime;
    53.  
    54.      
    55.      
    56.         //Debug.Log(rot_vel[1]);
    57.  
    58.         this._message = new OdometryMsg();
    59.  
    60.         if (_timeElapsed > ( 1f / publishMessageFrequency))
    61.         {
    62.             // coverting from unity to ros axis
    63.             ang_pos = base_link.transform.rotation.To<FLU>();
    64.             lin_pos = base_link.transform.position.To<FLU>();
    65.             rot_vel = rb.angularVelocity.To<FLU>();
    66.             lin_vel = rb.velocity.To<FLU>();
    67.          
    68.             //Debug.Log(rot_vel);
    69.  
    70.  
    71.             uint sec = (uint)Math.Truncate(this._timeStamp);
    72.             uint nanosec = (uint)( (this._timeStamp - sec)*1e+9 );
    73.             // Debug.Log(sec);
    74.             this._message.header.stamp.sec = sec;
    75.             this._message.header.stamp.nanosec = nanosec;
    76.             this._message.header.frame_id = frameId;
    77.  
    78.             this._message.child_frame_id = base_link.transform.name;
    79.  
    80.             this._message.pose.pose.position.x = lin_pos.x;
    81.             this._message.pose.pose.position.y = lin_pos.y;
    82.             this._message.pose.pose.position.z = lin_pos.z;
    83.  
    84.             this._message.pose.pose.orientation.x = ang_pos.x;
    85.             this._message.pose.pose.orientation.y = ang_pos.y;
    86.             this._message.pose.pose.orientation.z = ang_pos.z;
    87.             this._message.pose.pose.orientation.w = ang_pos.w;
    88.  
    89.             this._message.twist.twist.linear.x = lin_vel.x;
    90.             this._message.twist.twist.linear.y = lin_vel.y;
    91.             this._message.twist.twist.linear.z = lin_vel.z;
    92.  
    93.             this._message.twist.twist.angular.x = rot_vel.x;
    94.             this._message.twist.twist.angular.y = rot_vel.y;
    95.             this._message.twist.twist.angular.z = rot_vel.z;
    96.  
    97.             ros.Publish(this.topicName, this._message);
    98.  
    99.             this._timeElapsed = 0;
    100.             this._timeStamp = Time.time;
    101.         }
    102.     }
    103. }
    104.  
    105.  
    I made it to publish 100hz
    upload_2022-1-28_19-26-29.png

    But hardly its reaching out to 16 hz:
    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. ActiveSim

    ActiveSim

    Joined:
    May 10, 2019
    Posts:
    59
    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).