Odometry channel frequency issues

[cburbridge]

There seams to be a delay of about 400ms between commanding a velocity, and the odometry of the robot reporting that the current velocity is the one that was commanded.

The velocity being commanded is not outside of the acceleration limits of the robot. This is nothing to do with ROS topics - it is the time between calling the MIRA service callService<void>("/robot/Robot", "setVelocity", speed); and the value reported on the MIRA /robot/Odometry channel.

Here is a plot of the commanded (green) vs odeometry reported(blue) velocities.

The delay is more apparent when zoomed in:

Since the reported velocity looks to match the commanded, only offset by 400ms, could this be a problem with the odometry channel? I have tried having the subscription running in its own thread, but this has not effect. The frequency of the odometry messages remains:

In this plot we can see that the odometry messages usually have just over 50ms between each, except the occasional hiccup where it has maybe missed one? Regardless of the delayed messages, I would not expect this plot. If the messages are generated at 20Hz - so one every 50ms - then should there not be a mean of 50ms between messages? For example, if I received the message due at t=50ms at t=55ms (5ms late), then the next message due at t=100ms at t=102ms (2ms late) then the time between these is only 47ms. In this plot there is no "time between" of less than 50ms, yet there are quite a few more than 50ms. So is the frequency of the odometry channel not constant?

The last plot is the time between velocity commands sent by the move_base control loop. Here the target frequency is 10Hz, and the mean is 14 microseconds off 10Hz.

[cburbridge]

Any idea @creuther ? How is the odometry channel implemented in the robot firmware / MIRA drivers?

[creuther]

I'll look into it but I'm afraid everyone who could help me @ MetraLabs is on business trips today.
On 9 Nov 2015 14:34, Chris Burbridge notifications@github.com wrote:Any idea @creuther ? How is the odometry channel implemented in the robot firmware / MIRA drivers?

—Reply to this email directly or view it on GitHub.

[cburbridge]

strands-project/scitos_robot#53 mentioned the option "CalculateOdometryFromDistance". I tried this, and it results in not as good odometry but faster with a message frequency I had expected:

However, even with this the odometry reports a current velocity lagged behind the commanded:

[creuther]

So within the SCITOSDriver software there is no built-in limiting or smoothing of the incoming odometry message rate, so it basically just takes them as they come. I am quite certain that the frequency of the odometry channel isn't entirely constant, but I lack the insight into the MCU / microcontroller software level to verify that assumption.

We actually did a similar plot quite a while ago, plotting odometry against commanded velocities, and found some sort of almost constant offset as well. Though we did it in the context of trying to find out the cause of oscillations in the drive characteristics.

But a general thing: Am I mixing something up completely or is a certain constant offset always to be expected in these PID-like control scenarios? Doesn't the defined maximum acceleration actually bound how fast you can reach a certain velocity v_t given your current velocity v_0 ? As the odometry simply reports at what rate the wheels are spinning, I would expect to see curve plots exactly like that. But whether the delay is supposed to be 400ms, that is a different question.

[cburbridge]

Yes, there will always be a slight unavoidable delay, but not 400ms. I think the actual velocity (odometry reported) should respond near immediately to a change in commanded speed, but not necessarily immediately be at the desired speed. I get the impression that in the above plot on the first acceleration of the robot, when there is a wobble in green line there is a corresponding wobble in the blue line. This wobble in the blue is the controller responding to the command, which is ~400ms later. Surely this should be much quicker?

[creuther]

Still waiting to hear back about the microcontroller side of things re: inconsistent odometry frequency, but I might have a possible cause for some part of the 400ms delay.

Since MCU firmware 1.8.4 there is support for a ramp function regarding the acceleration, and apparently the default value will cause some ramping. There is a parameter AccelerationUpdateInterval in the SCITOSDriver to set the steepness of the ramp.

Can you try adding this

<!--This value defines how often the acceleration or deceleration of the robot can be updated. Small values result to a steeper ramp. Bigger values leads to a more flat ramp and a more soft movement. -->
<AccelerationUpdateInterval>1</AccelerationUpdateInterval>

to the file SCITOS-G5.xml within the MainControlUnit tag? You should also be able to manipulate it like any other property of the MainControlUnit using the above mentioned property name.

It would be very interesting to see the difference this parameter has on the cmd_vel vs odom_vel plot.

[cburbridge]

It looks like this has made no difference...

[cburbridge]

Here is a plot of the difference between the time stamp from a received odometry message and the time that the message was received:

It follows a strange recurring pattern where the odometry gets further out of date, then resets itself at about 50ms. The difference is always in ~ 10ms increments.
The increase in delay in the middle occurs when the robot is being commanded with velocities, the other parts are where the robot is idle. So driving the robot increases the delay in getting the odometry data...

There are even a couple of negative differences, where the odometry is in the future.

@creuther :-) any idea ?

[g-gemignani]

Hello,

I have been talking with @cburbridge about this issue that we are also experimenting here at Magazino. I am posting our problems to help out the debugging process. I think that there might be some problems either in the robot firmware or in the MIRA software. Below you can find the reason why I believe this.

Here at Magazino, we are working with a Scitos X3. While debugging our navigation system we noticed a weird jittering in our laser scans. These two videos should show you what I mean with that. We are reading the values of the laser through the sick300 ros node, but that does not seem to be the problem since, as it can be seen from one of the videos, the jitter is introduced by the odom transform published by the ScitosDrive.cpp node of the scitos_driver ros package. Also, this jitter seems to be related to a timing problem. In fact, the odometry messages are arriving at a rate that is not constant.

Today, I spent the morning trying to narrow down the problem. I first checked if mira and ros time were synced. I found out that mira truncates the timestamps to the microsecond but this does not explain a delay of the order of tens of milliseconds. I then started to print the time difference between the odom messages timestamps and ros::Time::now in the odom_callback called by the odometry authority subscription in ScitosDrive.cpp. When printing the time difference at the beginning of the function I obtained a "chainsaw trend" that you can see below.

If I am not wrong, this means that the there is a delay that is introduced before this point.

Next, I played with the optional parameters of the subscriptions initializer hoping that, by changing them, something would change. However, nor setting the independentThread flag to true nor increasing the storageDuration changed anything. I also tried to increase the spinning time of ScitosG5 to 100 but that also didn't affect the result. At this point, I believe it's a problem in Mira.

I hope that this information can help out with the debugging process! :)

[creuther]

Thank you for that really valuable piece of debugging information! We are already looking into the issue, hopefully we can present some progress updates or even solutions soon!

[creuther]

Ok, here is the latest. Please forgive me if some things aren't completely clear or sensible, I am not a hardware guy and am only relaying information for the most part.

In our Main Control Unit (MCU), i.e. the low level controller, there are two asynchronous processes for calculating and transmitting the odometry. They aren't synchronized to each other so the sending process will slowly drift away from the calculating process, resulting in the observed jigsaw pattern. The actual timestamp of the odometry messages is still accurate (caveat: see below) as it is the hardware timestamp at time of calculation-

Furthermore, the internally used hardware timestamp only has a 10ms accuracy and is also being sent asynchronously. Therefore this introduces "allows" timing errors of up to 20ms. This could lead to the observed effects of measuring odometry slightly in the past or future.

We tested a firmware version where the odometry is sent as soon as it is calculcated, and the jigsaw discrepancy pattern observed above is gone.

As for the jittering: If you set the OdometryInterval to 10ms, which is the actual interval of motor ticks received in the hardware, the jitter decreases but doesn't disappear. Why this jitter persists is what we are currently looking into. It could be that the 10ms hardware timestamp accuracy is too coarse, or that there is a problem with the synchronization of hardware to software timestamps (done by the SCITOS driver), or that there is another dead time hidden somewhere we haven't found yet. Another factor is that the accuracy of the laser timestamp isn't completely known either (internally they increment a counter with every scan ~ about every 40ms).

I will keep this updated.

PS: Does the ROS range scan visualization interpolate the laser pose for the laser timestamp? If not, then that will introduce considerable jitter.

[g-gemignani]

Hey Christian,

Thank you for the update. As you suggested I changed the OdometryInterval to 10ms and it looks like we got some improvements (even though it's really hard to tell). Is it possible to have the changed firmware to test it out here? Regarding your question about the visualizer, as far as I know, ROS should interpolate the laser pose.
Let me know if I can help out in any way with the debugging. Thank you again for the update!

[g-gemignani]

Any news about this issue @creuther?
Thank you for your time!

[creuther]

Sorry @g-gemignani no more news at the moment. We unfortunately don't release experimental firmware, but a new firmware version including the fixes should be released within the next 7-10 days. As soon as it's released I'll let you know how to update!