#!/usr/bin/env python2
import rospy
from sensor_msgs.msg import JointState


class ThrottleByDistance:
    """Filter joint_states msg, if no joint_value changes more than threshold.
    """

    def __init__(self):
        self.old_states = []
        self.throttle_value = rospy.get_param("~throttle_value", 0.1)
        self.pub = rospy.Publisher("joint_states_changes", JointState, queue_size=10)
        rospy.init_node("throttle_by_distance", anonymous=True)
        rospy.Subscriber("joint_states", JointState, self.callback)
        rospy.spin()

    def publish(self, msg):
        self.old_states = msg.position
        self.pub.publish(msg)

    def callback(self, msg):
        # Define old_states at start-up or if number of joints has changed
        if len(self.old_states) != len(msg.position):
            if self.old_states:
                rospy.logwarn("Number of joints changed!")
            rospy.loginfo("Throttling joint_states by change: \n %s", msg.name)
            return self.publish(msg)

        for idx, val in enumerate(msg.position):
            if abs(self.old_states[idx] - val) > self.throttle_value:
                return self.publish(msg)


if __name__ == "__main__":
    throttle = ThrottleByDistance()
