0

I have a LeafSystem that DeclarePeriodicPublish(0.07, 0).

This LeafSystem reads data from a rgbd camera, does some processing and save the result to an attribute of the LeafSystem instance. The LeafSystem also declares multiple output ports, which simply copy the value saved in the attribute to the output.

If I simulate this LeafSystem by itself, the behavior is as expected and DoPublish get called roughly every 0.07 seconds.

However, if I wire this LeafSystem to a custom manipulation_station, then DoPublish only get called every 0.3 second.

I am not sure how to debug this issue. I have monitored CPU usage during simulation and the python command is not even using half the number of cores, so I am guessing CPU thrasing is not the issue here.

Any suggestion on how to fix this? Or maybe a better way to implement this read data -> processing -> publish data to other LeafSystems behavior in drake?

Thank you!

Editted on June 4 2021:

The use case is as follows. My pipeline is:

Capture data from a real rgbd camera -> Perform some processing -> Send command to robot in simulation.

During demo, I'd like to show video of the robot in simulation side-by-side with the captured rgbd stream.

The robot in simulation is currently being captured by recording the computer screen.

However, rendering the incoming rgbd stream to screen is too slow. So I am keeping in computer memory the rgbd frame and their timestamp. After recording robot behavior, I then re-render the rgbd stream as a video using the timestamps and then merge this video with the recording of the robot to form one final video.

The time duration between two timestamps is 0.3 seconds. However, the processing of each rgbd frame only takes 0.05 seconds.

The timestamp is obtained using python native time.time() function.

Quan Vuong
  • 1,919
  • 3
  • 14
  • 24

1 Answers1

0

That decidedly sounds like a bug. A LeafSystem that declares a periodic event should have that event invoked at the regular period, regardless of what the rest of the Diagram is.

Is there any way you can share your code?

Sean Curtis
  • 1,425
  • 7
  • 8
  • Thank you for the quick reply! This is internal research code and we have a ddl to hit, so might not have the chance to isolate the issue and share with you reproducible snippet soon. But any suggestion on how to debug this on our side is highly appreciated. – Quan Vuong Jun 02 '21 at 16:14
  • Checklist: 1. You've declared the periodic event in your leaf system's constructor? Or have you overriden `DoPublish`? (The former is definitely preferred.) 2. I presume it would be sufficiently innocuous to share that declaration? 3. I presume you've got some logging system letting you know when the callback is being exercised. What is that? – Sean Curtis Jun 03 '21 at 17:14
  • 1. I have overriden DoPublish – Quan Vuong Jun 04 '21 at 18:22
  • 2. The declaration is self.DeclarePeriodicPublish( 0.06, .0 ) – Quan Vuong Jun 04 '21 at 18:23
  • 3. Please find the explanation of the use case and logging in the original question (editted) – Quan Vuong Jun 04 '21 at 18:24
  • Several things. 1. 99% of the time, overriding `DoPublish` is a mistake. 2. You seem to have a real-time expectation. The time stamp you should print out is `context->get_time()` (in your event handler). You're printing the "wall clock" time. Those two clocks are not necessarily in sync. The simple reality is that your simulation may be running much slower than real time (I'd guess about 1/4 real time). If you confirm the simulator time, then we know it's not a bug in the framework, but a question of "Is this running as fast as you need/think?" – Sean Curtis Jun 07 '21 at 13:32
  • Thank you for your comment! 1. That's great to know! Is there an alternative that you recommend? Is it ```DeclarePeriodicPublishEvent```? I was overriding ```DoPublish``` following the code here [1]. Just fyi in case you want to change this piece of code to use ```DeclarePeriodicPublishEvent``` so other folks do not follow the same pattern. 2. Thank you for the suggestion about ```context->get_time()```. Looks like a great debug tool. Will try it! [1] https://github.com/RobotLocomotion/drake/blob/master/examples/manipulation_station/end_effector_teleop_sliders.py#L169 – Quan Vuong Jun 07 '21 at 17:51
  • I've added an issue to clean up old issues that haven't kept up with the times: https://github.com/RobotLocomotion/drake/issues/15147 – Sean Curtis Jun 08 '21 at 18:32
  • The preferred mechanism, as you ask, is to invoke `DeclarePeriodicPublishEvent`. You provide a method pointer as the callback to do whatever work you want to have happen. – Sean Curtis Jun 08 '21 at 18:33