Question ros unity clock publisher issue

vermahrithik10 · Apr 2, 2022

I have been testing ros unity for last 3 months and I found a big problem with ros unity clock. I'm able to make ros unity navigation for ros1 but big issue is that TF can be very sensitive when it comes to timestamps but unity ros clock is not that accurate. Similar issue is raised for ros-sharp: https://github.com/siemens/ros-sharp/issues/91

I constant get errors like:

[ERROR] [1648887261.965850238, 280.732946302]: Global Frame: odom Plan Frame size 63: map

[ WARN] [1648887261.965958291, 280.732946302]: Could not transform the global plan to the frame of the controller
[ERROR] [1648887293.731323658, 306.266675524]: Extrapolation Error: Lookup would require extrapolation into the future. Requested time 306.266675524 but the latest data is at time 306.133342211, when looking up transform from frame [odom] to frame [map]
Click to expand...

My ros clock publisher (similar to Robotics-Nav2-SLAM-Example):

Code (CSharp):

using System;

using System.Collections;

using System.Collections.Generic;

using UnityEngine;

using Unity.Robotics.ROSTCPConnector;

using RosMessageTypes.Rosgraph;

using Unity.Robotics.Core;

public class ClockPublisher : MonoBehaviour

{

[SerializeField] private string _topicName = "clock";

private float _timeStamp = 0f;

private ROSConnection _ros;

private ClockMsg _message;

public uint sec , nanosec, temp_sec;

//private uint temp_nanosec;

double m_PublishRateHz = 200f;

double m_LastPublishTimeSeconds;

double PublishPeriodSeconds => 1.0f / m_PublishRateHz;

bool ShouldPublishMessage => Clock.FrameStartTimeInSeconds - PublishPeriodSeconds > m_LastPublishTimeSeconds;

void Start()

{

// setup ROS

this._ros = ROSConnection.GetOrCreateInstance();

this._ros.RegisterPublisher<ClockMsg>(this._topicName);

// this._ros.Subscribe<LogMsg>(sub_topic, get_start_time);

// setup ROS Message

this._message = new ClockMsg();

this._message.clock.sec = 0;

this._message.clock.nanosec = 0;

//temp_sec = sec = 0;

}

void PublishMessage()

{

// Debug.Log(Time.fixedTime);

// this._timeStamp = Time.fixedTime;

// sec = (uint)Math.Truncate(this._timeStamp);

// nanosec = (uint)( (this._timeStamp - sec)*1e+9);

var publishTime = Clock.time;

sec = (uint)publishTime;

nanosec = (uint)((publishTime - Math.Floor(publishTime)) * Clock.k_NanoSecondsInSeconds);

// temp_sec++;

// if (temp_sec >= 1000)

// {

// temp_sec = 0;

// sec++;

// }

// nanosec = temp_sec * 1000000;

this._message.clock.sec = sec ;

this._message.clock.nanosec = nanosec;

m_LastPublishTimeSeconds = publishTime;

//m_LastPublishTimeSeconds = Time.fixedTime;

this._ros.Publish(this._topicName, this._message);

}

private void Update()

{

if (ShouldPublishMessage)

{

PublishMessage();

}

}

}

To retrieve clock in other scripts( for pub sensor data):

Code (CSharp):

void Start()

{

clock = GameObject.Find("ROSConnectionPrefab").GetComponent<ClockPublisher>();

}

//to access the clock time

this._message.header.stamp.sec = clock.sec;

this._message.header.stamp.nanosec = clock.nanosec;

Please let me know how to make a proper ros clock as well as how to retrieve that clock data in other scripts.
I think there should be a proper git docs tutorial on ros unity clock like the ros unity frame. Ros Clock is most important for ros unity to work in synchronize.

mrpropellers · Apr 4, 2022

The times being published by Unity's clock are incredibly accurate, but you may be making bad assumptions about what the time published represents. By default, the example clock publisher publishes the simulation time at the point when the last frame was rendered, not the current wallclock time. This means that executing several clock publishes in the same frame will result in several identical clock messages. For the Nav2-SLAM-Example, this was sufficiently granular since all of our sensors were running in step with the rendering thread. If you are publishing anything during FixedUpdate, however, you may want to have your clock publisher access Time.fixedTimeAsDouble (https://docs.unity3d.com/ScriptReference/Time-fixedTimeAsDouble.html) which reports the time the last physics update occurred. FixedTimeAsDouble is the most accurate and granular time step you can publish to ROS without fudging numbers. In order to ensure your clock publisher's rate is as close to expected as possible, you could check whether to publish in both the Update and FixedUpdate calls, and use Time.realtimeSinceStartupAsDouble (https://docs.unity3d.com/ScriptReference/Time-realtimeSinceStartupAsDouble.html), which is unscaled and measured against current wallclock time, rather than simulation time, to determine whether another clock publish should occur.

From the looks of that error, though, the problem is likely in how your publishers/subscribers are set up. An "extrapolation into the future" error is not unique to Unity simulation - it will also happen in Gazebo or on a real world robot - and will occur any time a tf lookup executes against a stale TF tree or against a bad timestamp. You may be executing TF lookups against
Time.now()
in your subscriber instead of using the timestamp from the sensor message. Or, if you are publishing sensor messages and TF messages with the same timestamp at the same time, you may simply have a race condition where the sensor message is sometimes received by your subscriber before the TF message, and your TF tree hasn't been updated when your subscriber attempts to process the sensor message.

If it's the latter and not the former, you can fudge things in a number of ways to avoid the error: you can wait a frame to publish your sensor data, you can add code to your subscribers to tell them to wait to process a sensor frame until after you've received the newest clock and tf messages, you could change the clock publisher to publish
Clock.Now
instead of
Clock.time
(which would be less accurate, but should guarantee you have a clock time which is newer than the sensor timestamp), or you could simply change your TF lookup invocations to allow extrapolation into the future (although this is almost always not a good solution and may just hide upstream problems). The best solution is usually the one recommended in the tutorials (http://wiki.ros.org/tf/Tutorials/tf and Time (Python)) -- you may just need to wait for the TF tree to update before attempting your tf lookup.

vermahrithik10 · Apr 7, 2022

@mrpropellers Thanks for answering.
Lets consider a simple unity ros OdomertryPublisher code

Code (CSharp):

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; //contain ros OdometryMsg c# Class

using Unity.Robotics.Core; // for calling Clock.FrameStartTimeInSeconds variable

public class OdomertryPublisher : MonoBehaviour

{

ROSConnection ros;

ClockPublisher clock;

private OdometryMsg _message;

public string topicName = "odom";

public string frameId = "odom";

public GameObject base_link;

ArticulationBody rb;

private Transform _trans;

QuaternionMsg ang_pos;

PointMsg lin_pos;

Vector3<FLU> lin_vel, rot_vel;

Vector3 ang_vel_unity;

public float publishMessageFrequency = 100f;

private float _timeElapsed;

private float _timeStamp;

uint sec, nanosec;

// var publishTime;

double m_LastPublishTimeSeconds;

double PublishPeriodSeconds => 1.0f / publishMessageFrequency;

bool ShouldPublishMessage => Clock.FrameStartTimeInSeconds - PublishPeriodSeconds > m_LastPublishTimeSeconds;

// Start is called before the first frame update

void Start()

{

clock = GameObject.Find("ROSConnectionPrefab").GetComponent<ClockPublisher>();

ros = ROSConnection.GetOrCreateInstance();

ros.RegisterPublisher<OdometryMsg>(topicName);

rb = base_link.GetComponent<ArticulationBody>();

_trans = base_link.GetComponent<Transform>();

}

private void FixedUpdate()

{

if (ShouldPublishMessage)

{

Publish_Odom();

}

}

void Publish_Odom()

{

this._message = new OdometryMsg();

// coverting from unity to ros axis

ang_pos = base_link.transform.localRotation.To<FLU>();

lin_pos = base_link.transform.localPosition.To<FLU>();

rot_vel = (this._trans.InverseTransformDirection(this.rb.angularVelocity).To<FLU>());

lin_vel = (this._trans.InverseTransformDirection(this.rb.velocity).To<FLU>());

this._message.header.stamp.sec = clock.sec;

this._message.header.stamp.nanosec = clock.nanosec;

this._message.header.frame_id = frameId;

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

this._message.pose.pose.position = lin_pos;

this._message.pose.pose.orientation = ang_pos;

this._message.twist.twist.linear = lin_vel;

this._message.twist.twist.angular = rot_vel;

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

this._timeElapsed = 0;

this._timeStamp = Time.fixedDeltaTime;

m_LastPublishTimeSeconds = Clock.time;

}

}

how would you take timestamp message and publish it to ros. I found it quite hard to understand unity ros clock and to access it data and publish correct timestamp. Please help me out.

As well as its a humble request to unity team to make a proper git page tutorial on how to use unity time from the perspective of ros and publishing ros message with correct timestamp because incorrect timestamp can mess up everything.

I highly believe that unity has all capability to surpass all present day Ros Simulator (like Gazebo, Coppeliasim, Webots etc) in terms of high graphics realistic scenes. Just there are some basis ros need to be satisfy like ros lightweight sensors, ros publishing frequency etc.

mrpropellers · May 6, 2022

Apologies for not seeing this sooner but hopefully this info is better late than not at all:
Change line 62 of your ClockPublisher script from
var publishTime = Clock.time;
(time of the start of the frame) to
var publishTime = Clock.Now;
(exact wallclock time)
Change line 87's
Update
to
FixedUpdate
In the OdometryPublisher, you will need to use Unity's Time interface, because the clock example in the Nav2SLAMExample does not have an interface for getting the time of the last FixedUpdate. Populate your header with the time returned by Time.fixedTimeAsDouble using the same type of conversion to secs/nsecs that you did in ClockPublisher. This will work fine as long as your simulation is running in realtime.
We don't have a tutorial for managing simulation time because clock management is not an officially supported feature in our packages yet. I can't (as a rule) provide any transparency into our internal prioritization of feature work, but I can promise as a roboticist that has spent many years working with real-time autonomous systems in simulation, proper clock management is always on my mind

Search Unity

Question ros unity clock publisher issue

vermahrithik10

mrpropellers

Unity Technologies

vermahrithik10

mrpropellers

Unity Technologies

Search Unity

Unity ID

Useful Searches

Question ros unity clock publisher issue

vermahrithik10

mrpropellers

Unity Technologies

vermahrithik10

mrpropellers

Unity Technologies