#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
import math

def foo(new_vel, old_vel, step):
    if new_vel > old_vel + step:
    return old_vel + step
    elif new_vel < old_vel - step:
    return old_vel - step
    else:
    return new_vel

class Smoother():
    def __init__(self, lin_step = 0.01, ang_step = 0.1):
    rospy.init_node('vel_smoother', anonymous=True)

    self.last_twist = Twist()
    self.last_twist.linear.y = 0.0
    self.last_twist.linear.z = 0.0
    self.last_twist.angular.x = 0.0
    self.last_twist.angular.y = 0.0

    self.LIN_STEP = lin_step
    self.ANG_STEP = ang_step

    rospy.Subscriber("cmd_vel", Twist, self.smooth)
    self.pub = rospy.Publisher("cmd_vel_sm", Twist, queue_size=100)

    while not rospy.is_shutdown():
        rospy.spin()

    def smooth(self, data):
    self.last_twist.linear.x = foo(data.linear.x, self.last_twist.linear.x, self.LIN_STEP)
    self.last_twist.angular.z = foo(data.angular.z, self.last_twist.angular.z, self.ANG_STEP)

    self.pub.publish(self.last_twist)

if __name__ == '__main__':
    s = Smoother()