joint_states_appender.py

Go to the documentation of this file.

#!/usr/bin/env python
 
 
 
import rospy
from sensor_msgs.msg import JointState
 
prev_joint_states = None
pub = None
 
def callback(msg):
    global prev_joint_states, pub
    if not prev_joint_states:
        # initial time
        # just pass the input message to output
        pub.publish(msg)
        prev_joint_states = msg
        # force to use list instead of tuple.
        prev_joint_states.name = list(prev_joint_states.name)
        prev_joint_states.position = list(prev_joint_states.position)
        if len(prev_joint_states.velocity) > 0:
            prev_joint_states.velocity = list(prev_joint_states.velocity)
        else:
            prev_joint_states.velocity = [0] * len(prev_joint_states.name)
        if len(prev_joint_states.effort) > 0:
            prev_joint_states.effort = list(prev_joint_states.effort)
        else:
            prev_joint_states.effort = [0] * len(prev_joint_states.name)
    else:
        if len(msg.velocity) > 0:
            velocity = msg.velocity
        else:
            velocity = [0] * len(msg.name)
        if len(msg.effort) > 0:
            effort = msg.effort
        else:
            effort = [0] * len(msg.name)
 
        for name, pos, vel, ef in zip(msg.name, msg.position, velocity, effort):
            if name in prev_joint_states.name:
                # if name is already listed in prev_joint_states,
                # update value
                index = prev_joint_states.name.index(name)
                prev_joint_states.position[index] = pos
                prev_joint_states.velocity[index] = vel
                prev_joint_states.effort[index] = ef
            else:
                # if name is not yet listed in prev_joint_states,
                # append name and position to prev_joint_states
                prev_joint_states.name.append(name)
                prev_joint_states.position.append(pos)
                prev_joint_states.effort.append(ef)
                prev_joint_states.velocity.append(vel)
        if prev_joint_states.header.stamp < msg.header.stamp:
            prev_joint_states.header.stamp = msg.header.stamp
        pub.publish(prev_joint_states)
        
if __name__ == "__main__":
    rospy.init_node("joint_states_appender")
    pub = rospy.Publisher("joint_states_appended", JointState, queue_size=1)
    sub = rospy.Subscriber("joint_states", JointState, callback, queue_size=1)
    rospy.spin()