High latency between roscontrol and Roboteq node

edit

I've wrote a controller code using ros_control_boilerplate as base, and connected it to the Roboteq driver using ROS topics. After that, I loaded a diff_driver_controller and was able to control the robot using twist messages, but looking at the graph generated by rqt_plot, there's a delay of more than 100ms between the twist message (ros_control) and the setpoint message (roboteq driver).

Both are running on the same machine, and using 100Hz control loop. Is there any way to identify the bottleneck, so I can fix it? I was expecting a delay of a few ms, at worst.