#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry

class SubThenPub:
	def __init__(self, init_distence, vel_lambda):
	        self.init_distence = init_distence
	        self.vel_lambda    = vel_lambda
		self.distence_diff = init_distence
	        self.my_vel = 0
        	self.last_vel = 0
		self.__pub_ = rospy.Publisher('/robot_1/cmd_vel', Twist, queue_size=10)
		self.__sub_ = rospy.Subscriber('/robot_0/odom', Odometry, self.callback)

	def callback(self, data):
	        self.distence_diff = (data.twist.twist.linear.x - self.last_vel) / 50 + self.distence_diff

        	acceleration = self.vel_lambda * (data.twist.twist.linear.x - self.last_vel) / self.distence_diff
		self.my_vel += acceleration / 50

		vel_msg = Twist()

		vel_msg.linear.x = self.my_vel

		self.__pub_.publish(vel_msg)
	        self.last_vel = self.my_vel
        
		rospy.loginfo("pub velocity:[%0.2f m/s]",vel_msg.linear.x)

def main():
	init_distence = float(input('please input init distence between two cars (m):'))
	vel_lambda    = float(input('please input vel lambda of the model:'))
	rospy.init_node('follow_r0', anonymous=True)
	SAPObject = SubThenPub(init_distence, vel_lambda)
	rospy.spin()

if __name__ == "__main__":
	main()