Single stepping - ways to speed it up?

edit

"If I understand your architecture correctly, your Simulink process sends a world control over the network in between each step." That's correct. In fact, I do a bit more: I first send a topic message containing new joint angles. Then I send a world control message. The plugins associated with the robot model have callbacks registered to the WorldUpdateEnd event,which generate topic messages containing the various sensor values at the end of the time step which get sent back to Simulink.

If I strip this down to the absolute bare minimum - no sensors other than the joint angles reported by the links - then the system runs at around 20x slower than real time. "Do you know how fast you can do this in an empty world (without an models)? This could give you an idea of the maximum performance you can expect." I haven't tried that, but it's a good idea. I will work out the code and report back.

"Can you generate code from Simulink and run it from inside a Gazebo plugin?" Unfortunately, that's probably not viable for us. We're using this to develop and validate a very complex set of control laws, most of which are written in Simulink. All of it eventually gets run on a SpeedGoat. Ultimately we'd like use the gazebo sim to run hardware-in-the-loop tests. So we really do need the Simulink code and gazebo to be separate.

The update rate we want out of gazebo is between 1-2 kHz; but the World Control messages seem to get processed at only about 50 Hz. I think it should be possible to send protocol buffer-based messages through a TCP port at far faster rates than that, and I'm trying to figure out what else might be the cause of the slowdown.

"Do you know how fast you can do this in an empty world (without an models)?" Turns out that this doesn't really work the way I expected. Without the feedback - eg, without a topic being returned from gazebo to simulink reporting some sort of sensor data - Simulink goes ahead and issues 1000 world control topics as fast as it can, and most of those get silently dropped somewhere in the transport layer...

..., because 1000 such topics should cause gazebo to step one full second, but it turns out that gazebo only steps forward about 0.2 seconds. Which means that 80% of the world command messages are getting lost. The upside, though, is that this happens nearly instantaneously.

Attempting to be clever, I added a boost::thread:: sleep command inside my simulink code as a placeholder for a nonexistent sensor feedback topic, so I can run with no loaded model. Running this way I can get to just about real time.

With the model loaded but everything else the same - so only a world command topic is being passed through the transport layer - I can get the speed to about 2x slower than real time without any topic messages being lost. This implies that about 90% of my overhead is tied up in sending and receiving sensor topics. I'm also curious about how TCP-based topics messages could be getting silently dropped.