Publish too slow

edit

This is for the dynamixel_controllers package ( http://wiki.ros.org/dynamixel_control... ), but I don't think it's specific to this particular package. The package is installed from source in my catkin_workspace, not from binaries.

Ok, I have 5 callbacks (JointController.process_motor_states, 5 instances of JointController) that subscribe to a topic (MotorState.msg) that is being published at 20Hz, so 0.05s. Each of the callbacks also publishes a message (JointState.msg). Now timing the callbacks I see that summing up all 5 takes longer than 0.05 s and is creating a bottleneck. So what is happening is that process_motor_states ends up falling way behind and gets further and further out of sync as time goes on. Further inspection of the process_motor_states shows that the bottleneck is the publishing of the message (JointState.msg), which itself can take up to 0.05s. So in the code snippet below, the issue is the last line.

def process_motor_states(self, state_list):
    if self.running:
        state = filter(lambda state: state.id == self.motor_id, state_list.motor_states)
        if state:
            state = state[0]
            self.joint_state.motor_temps = [state.temperature]
            self.joint_state.goal_pos = self.raw_to_rad(state.goal, self.initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)
            self.joint_state.current_pos = self.raw_to_rad(state.position, self.initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)
            self.joint_state.error = state.error * self.RADIANS_PER_ENCODER_TICK
            self.joint_state.velocity = state.speed * self.VELOCITY_PER_TICK
            self.joint_state.load = state.load
            self.joint_state.is_moving = state.moving
            self.joint_state.header.stamp = rospy.Time.from_sec(state.timestamp)

            self.joint_state_pub.publish(self.joint_state)

Here's the JointState.msg:

Header header
string name         # joint name
int32[] motor_ids   # motor ids controlling this joint
int32[] motor_temps # motor temperatures, same order as motor_ids

float64 goal_pos    # commanded position (in radians)
float64 current_pos # current joint position (in radians)
float64 error       # error between commanded and current positions (in radians)
float64 velocity    # current joint speed (in radians per second)
float64 load        # current load
bool is_moving      # is joint currently in motion

There is one subscriber for each of the instances of this publisher:

rospy.Subscriber('/%s/state' % controller_name, JointState, self.state_update)

If the subscriber callback for the JointState.msg is simply a pass statement so:

def state_update(self, joint_state):
      pass

Then we still have the bottleneck. If I remove the single subscriber then there is no bottle neck.

Any help is welcome. I don't think I'm doing anything weird here.