ROS not calling callbacks fast enough

edit

EDIT:

Here's a basic talker/listener example that reproduces the problem, if anyone else wants to try it. The talker is sending msgs of type gazebo_msgs::LinkStates, at 1kHz. When less than 5 links are in the message, the listener callback is called at 1kHz. When there are more than 5 links, the listener callback only gets called at ~170 Hz.

Original Post:

There is something wrong with how ROS is calling subscriber callbacks at a much slower rate than msgs are being published. Here's a minimal example.

class DataHandler
{
private:
  ros::NodeHandle nh;
  ros::Publisher test_pub;
public:
  DataHandler() { test_pub = nh.advertise<geometry_msgs::PoseStamped>("test",10); }
  void callback(const gazebo_msgs::LinkStatesConstPtr& msg)
  {
    test_pub.publish(geometry_msgs::PoseStamped());
  }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "gazebo_to_ros");
  ros::NodeHandle nh;

  DataHandler callbacks;
  ros::Subscriber linkSub = nh.subscribe("gazebo/link_states",1,&DataHandler::callback,&callbacks);

  ros::spin();
  return 0;
}

The topic "gazebo/link_states" is being published to at 500 Hz (using rostopic hz at the command line). The code in the callback takes a trivial amount of time to run (using std::chrono, less than 1ms => can run >1000Hz). The topic "test" is only being published to at ~70 Hz. Time between callback calls fluctuates between 1e-5 s and 0.04s, => 100kHz to 25 Hz, yielding an average of ~70 Hz.

Resource monitor shows cpu usage at less than 50%. Even setting high priority for this process changes nothing, so I don't think its a bottleneck in the hardware.

I'm on Ubuntu 16.04 LTS, with ROS Kinetic. Anyone have any idea at what I should look at next to help troubleshoot? Thanks for your help.