def joint_states_cb()

in src/rosbridge/fetch_proxy.py [0:0]

• 27 lines of code
• 11 McCabe index (conditional complexity)

    def joint_states_cb(self, data):
        """
        Handle a joint_states message. Messages can have subsets of the joints, hopefully non-overlapping.
        """
        t0 = time.time()
        for i in range(len(data.name)):
            name = data.name[i]
            jni = self.joint_name_map.get(name, -1)
            if jni >= 0:
                self.cur_joint_pos[jni] = data.position[i]
                self.cur_joint_vel[jni] = data.velocity[i]
                self.cur_joint_effort[jni] = data.effort[i]

        with self.timeseq_mutex:
            if self.timeseq:
                channel, channel_type = ((data.name[0] == 'l_wheel_joint' and ('robot_joints', 'FetchRobotJoints')) or
                                         (data.name[0] == 'l_gripper_finger_joint' and ('gripper_joints', 'FetchGripperJoints')) or
                                         (None, None))
                if channel:
                    state = dict([(jn, {
                        '__type': 'JointState',
                        'pos': data.position[i],
                        'vel': data.velocity[i],
                        'effort': data.effort[i],
                        }) for i, jn in enumerate(data.name)])
                    state['__type'] = channel_type
                    state['rxtime'] = t0
                    self.timeseq.add(data.header.stamp.to_sec(), channel, state)
                if 0:
                    t1 = time.time()
                    print '%0.6f+%0.6f stamp=%0.6f (age %0.6f) from %s' % (t0, t1-t0, data.header.stamp.to_sec(), data.header.stamp.to_sec()-t0, channel)