High frequency operation

[andre-nguyen]

Salut les gars,

Model: Ellipse Micro A with the development board OS: Ubuntu 18.04.2 LTS ROS version: Melodic

thanks for the good work on this driver. Out of curiosity, have you guys been able to work with streams at 200Hz? At first I had set the IMU data to 200Hz but it was only coming it at 50Hz. After some poking around I found out you can set the serial port to low latency:

setserial /dev/ttyUSB0 low_latency

Now I almost have a proper 200Hz however I can see some dropped messages from time to time.

I'm wondering if we should multi thread this driver and have 1 thread operating the uart and pusing to a ring buffer the raw SBG messages while another thread reads the buffers and converts to ROS messages.

Any ideas?

[andre-nguyen]

I rewrote the driver on my end with the latest sbgecom and the problem disapeared.

[nicolaje]

Hi @andre-nguyen,

do you have an implementation available for the public ?

We are also facing issues operating at high latency, or operating on the SBG Ellipse Micro.

Best,

Jeremy

[andre-nguyen]

@nicolaje I can ask if I can make my implementation public. But here is the main problem with this driver, this is especially true if you are using the dev board or an interface that goes through an FTDI adapter. Because of the buffer on serial ports, when you call sbgEComHandle you might end up processing multiple IMU logs. Since you stamp everything with ros::Time::now() you end up with multiple packets with the same stamp. Furthermore, what I thought were dropped packets was actually two outgoing packets stuck very close together in time.

Here are few suggestions:

• Boost your baud rate to 921600
• Run the driver as fast as possible (I set mine to 400Hz, but maybe 200Hz is fine).
• If you're going through FTDI, open the port as low latency by modifying the SBGEcom serial interface

+ #include <linux/serial.h>
+ ...
  //
  // Define 
  //
  if (tcsetattr((*pSerialHandle), TCSANOW, &options) != -1)
  {                             
+   struct serial_struct serial;
+   ioctl(*pSerialHandle, TIOCGSERIAL, &serial);
+   serial.flags |= ASYNC_LOW_LATENCY;
+   ioctl(*pSerialHandle, TIOCSSERIAL, &serial);
+ 
    //
    // The serial port is ready so create a new serial interface
    //
    pHandle->handle = (void*)pSerialHandle;
    pHandle->type = SBG_IF_TYPE_SERIAL;
    pHandle->pReadFunc = sbgInterfaceSerialRead;
    pHandle->pWriteFunc = sbgInterfaceSerialWrite;

On my side I'm using the Ellipse Micro2 A so I have no GPS timestamps. I can condition the clock with a PPS signal but I don't think I can give it the associated UTC time. So I also have external hardware that captures the Sync out pulses to re-timestamp the IMU messages for precise sync. For now it works well enough that I have built a working visual-inertial setup, but it's unclear to me how good the sync is with my other sensors...

[ErwanCL]

Hello guys,

Because of the buffer on serial ports, when you call sbgEComHandle you might end up processing multiple IMU logs. Since you stamp everything with ros::Time::now() you end up with multiple packets with the same stamp.

Indeed, this is a problem for timestamping the messages. One solution will be to create a time reference between UTC / GPS time and the device timestamp. Every SBG log has a timestamp, corresponding to the time since the sensor powered up.

In case you're using a device with GPS timestamp, we can define a direct dependence between the device timestamp and the GPS one, using the corresponding SbgLog. In case of an Ellipse Micro for instance, where you don't have the GPS timestamps, we can base timestamp on the ros::Time::now() function, to create the dependence at the first log received.

By doing this, even if you handle several logs with the sbgEComHandle, the timestamp will be computed again to avoid several packages with the same stamp.

I will work on a better way to handle timestamping for logs very soon.

Best regards,

Erwan

[andre-nguyen]

Hi @ErwanCL

I agree, that seems to make sense if you have one of the GPS models (I don't). I think what could also work in addition to opening the port as low latency is to have the serial port read in blocking mode in a seperate thread. That way you don't need to spin ros a lot for nothing and you would get the bytes ASAP.

[ErwanCL]

@andre-nguyen

I agree, multi-threading this driver for the read operations could be a good idea as well. I will soon test the driver on an Ellipse-micro and development board, to be as close as possible to your setup.

But as a first step, I would like to notice that the driver is working on two frequency loops : the Sbg device one, and the ROS one. As you noticed it, the sbgEComHandle may sometimes handle several messages, due to serial port buffer, or else, but the ROS frequency loop will publish messages, only one for each SbgLogType.

I just ran a simple test at 200Hz with an Ellipse2A, with just the IMU data output. On the screenshot, you will see the :

• "Received logs", corresponding to every time the sbgEComHandle received a IMU log
• "Published logs", corresponding to every time the ros node published an IMU SBG-ROS message.

We can see that we have some "dropped" message as you noticed it in your first post, but in fact, these are just logs that haven't be properly handled and published.

As a first solution, I would like to improve the message handling/publishing, by just publishing message as soon as they're received from the Sbg callback. By doing that, all the messages will be handled, and you shouldn't see "dropped" messages anymore.

[ThomasLeMezo]

@andre-nguyen @ErwanCL In the driver there is no need to use the spinOnce() function as ROS normally publish a message as soon as the publish() function is used (see here). The spin function is only needed for subscribers. A better integration between the Sbg callback and the publication of a new message is a very good improvement.

[ErwanCL]

Hello,

I just pushed some changes about the message publishing. I created an other branch, in order for you to be able to test the changes on your integrated platforms, before changing the driver for good.

The main idea behind my changes is to split the functionalities of the driver :

• a class to wrap the SBG logs to ROS messages
• a class to handle the publishers, by initializing them and publish corresponding messages
• the ellipse class, which controls the link between the functionalities.

A better integration between the Sbg callback and the publication of a new message is a very good improvement.

For that, in the sbgECom protocol defined in the Ellipse class, the callback function will automatically sent the received log to the publisher class, which will handle the log, according to the message type, and if the publisher is defined for this specific log.

For timestamping the logs, the header timestamp is defined as the ros::Time::now function, as it was, for now. I am still thinking about a better and proper way to do that.

Do not hesitate if you see any mistakes, or improvements. I hope these changes will help you for you high frequency issues, and I will working on cleaning up the driver, and find a solution for the timestamps.

Best regards, Erwan

[ThomasLeMezo]

Bonjour @ErwanCL , Thanks for the update ! I haven't test the driver yet but it seems from the code that it will work better now. I have some comments or questions:

• Concerning the functions like createRosVector3, it could be interesting to declare them as "inline" as it would reduce the cost of their call. I am wondering also if it is faster to have an instance variable for all messages instead of instantiate them each time a message is published.
• In MessagePublisher::MessagePublisher(void) function, why is it necessary to shutdown() publishers? Normally they should be down at this stage, or I miss something.
• The getOutputFrequency function is equivalent to the map in ellipse.h (l.184), so I would only keep one of the function.
• The ros::spinOnce(); function in main.cpp is useless and will take time. Messages are published as soon as the publish() function is called. We don't have subscriber so it is not necessary to keep it.

Best, Thomas

[andre-nguyen]

@ErwanCL Peux-tu faire une PR de messagePublisherRework à devel so we can move the discussion there?

[francisc0garcia]

Hi, First, thanks for your great job developing this driver! Second, I have tested the branch messagePublisherRework and it works fine, but I think there are some typos in file ellipse.cpp lines 66 to 68.

m_log_mag_calib should be changed to m_log_ekf_euler, m_log_ekf_quat and m_log_ekf_nav respectively.

Thanks!

[ErwanCL]

Bonjour Messieurs,

Concerning the functions like createRosVector3, it could be interesting to declare them as "inline" as it would reduce the cost of their call. I am wondering also if it is faster to have an instance variable for all messages instead of instantiate them each time a message is published.

Modern compilers do optimisation on themself, so the need of inline functions are not that much recommended anymore, so I don't think this is usefull to do that here. For the instance variable, it means that you would store an instance of every kind of message in your class, for not really using them. So by instanciating a message each time is needed, you create only the corresponding message, and as soon as it is published, it is destroyed. For instance, if you use the driver only for IMU data message, you will store 20 messages in your class for not using them. That's why I don't think it is necessary to have messages defined directly in the class.

In MessagePublisher::MessagePublisher(void) function, why is it necessary to shutdown() publishers? Normally they should be down at this stage, or I miss something.

I did some tests, with other features, and I thought it was required, but you are right, these calls are useless, and have to be removed.

The getOutputFrequency function is equivalent to the map in ellipse.h (l.184), so I would only keep one of the function.

Once again, you are right, keeping both is useless. I defined it in the publisher class because it is directly related to its functionality. I think the switch/case will be more efficient. https://stackoverflow.com/questions/931890/what-is-more-efficient-a-switch-case-or-an-stdmap

The ros::spinOnce(); function in main.cpp is useless and will take time. Messages are published as soon as the publish() function is called. We don't have subscriber so it is not necessary to keep it.

It is indeed useless to keep it, unless we decide to define some subscribers.

Thanks for your help @ThomasLeMezo, I will fix it !

@ErwanCL Peux-tu faire une PR de messagePublisherRework à devel so we can move the discussion there?

@andre-nguyen Sure I can do that. I guess it will be easier to compare with your own work.

@francisc0garcia, thanks for your feedback. Great to hear that the rework is working for you as well. I did some typos mistakes indeed on the logs, that will be fixed as well !

[andre-nguyen]

Gentlemen, I have published my internal hacky version of the driver https://github.com/andre-nguyen/sbg_driver its not something usable by anyone in the public because it is tightly set to my requirements and ties in with external time synchronization mechanisms but maybe it can give you a few ideas.

[ErwanCL]

The recent changes on the driver fix the lost messages.

As your version @andre-nguyen has nothing to see anymore with the driver, we can close the issue here.