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]

https://github.com/strands-project/scitos_robot/issues/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!

[g-gemignani]

Actually @cburbridge a firmware update in the next few days is the exact news that I was hoping for. Thank you so much for the update! :)

[marc-hanheide]

@creuther is this firmware update happening any time soon? We are currently observing strange issues with the TUW robot, having some delays that could be related. @Jailander @cdondrup discovered this...

[creuther]

We finally released firmware updates for the Main Control Unit (MCU) and IMU, which should fix the issues mentioned in my earlier comments. Sorry that it took so long.

Here is a quick walkthrough on how to update.

1 Download files attached and verify MD5 sums.

e71c63e5d3146a5bc9338c1e0c7e3541 T001E183_MainControlUnit_HG2_v1.8.12.sfw 756a367404c40ae5b041b77306713035 T001E305_MotionTrackingModule-v1.2.0.sfw

2 Locate firmware update tool

The tool scitosFirmwareLoader should either be in your $PATH, or you can find it in the bin/ directory of your MIRA installation.

3 Install IMU firmware

Run the scitosFirmwareLoader like this:

scitosFirmwareLoader /dev/ttyUSB2 T001E305_MotionTrackingModule-v1.2.0.sfw

This will find all active CAN modules and compare them to the hardware module signature stored in the .sfw file. After the scan is completed, you will be presented with a list of CAN modules, which should include the Motion Tracking Module (IMU). The scitosFirmwareLoader should offer to select the number of this module automatically, e.g.:

Do you want to select module 5? (y/n)

Verify that this is the correct module and then perform the firmware update by pressing y and Enter. Do not turn off the robot or interrupt the software while the firmware is being installed.

4 Repeat for MCU firmware (T001E183_MainControlUnit_HG2_v1.8.12.sfw)

5 After MCU firmware is updated, you will be prompted to shut down the computer by pressing Enter. After the PC has shut down, you have to turn off the ignition key switch of the robot to turn off any kind of power to the system. Only after this (and turning it back on) will the MCU firmware be updated.

6 Everything should work as before, sans odometry timing issues.

@Jailander and @cdondrup, I have done this for Bob as well but I am lacking Chris' insight into the ROS stack to perform any kind of meaningful tests. Could you perform the update as well and test whether it helps?

SCITOS_Firmware_Update_220216.zip

[cdondrup]

We will try to update Linda and perform a few tests yes. But we also lack Chris' elaborate testing suit. But I'm sure that by involving @gestom and @nilsbore we will be able to verify if it solves the issue.

Thank you!

[marc-hanheide]

Thanks for this. @jailander can you please try this on Linda and then, after it is confirmed working, should update Werner as well.

[Jailander]

yes we will try it on Linda as soon as possible, however, are we really sure we want to change this on Werner? after the time it took to detect and fix the overshooting problem we had at the pre-deployment I am not sure how confident we are that we can asses or in the worst case solve its performance remotely, any thoughts @marc-hanheide ? @creuther is there any way to backup the current firmware on the robot just in case?

[creuther]

Unfortunately not @Jailander, but if you send me the version you currently have on the robot I can provide you with the firmware file for that. Should be 1.8.9 for the MCU and 1.0.2 for the IMU, but the scitosFirmwareLoader will be able to tell you which version it really is.

[Jailander]

Ok that also works, I think @cdondrup already knows the version number, I'll get him to give it to you in a few minutes if he does

[cdondrup]

No, sadly I don't. We had a few problems with segfaults when trying to update the firmware so I don't know the version.

[nilsbore]

1.8.9 was the driver for the MCU on Werner. Don't know about the IMU. The segfaults were just on Linda I think.

[nilsbore]

I am sadly not able to test this atm, we are sending another power board to Metralabs for inspection.

[nilsbore]

I am happy to help out if you need any advise or anything, as I understood it, @cburbridge tested the odometry timings outside the ROS stack but if they look good even within we should be all set. Maybe we can e.g. check that the laser scan is moving more continuously with respect to the map (when rotating) as compared to the previous stuttering pattern? I think this triggered the initial investigation.

[Jailander]

yes sorry my mistake, I knew at some point we found the version number I just didn't remember how or who, I am preparing some stuff for IROS today, as soon as I get some time I'll test this on Linda's arena and then we can decide what to do with Werner depending on input from @marc-hanheide