00001 #! /usr/bin/env python 00002 import rospy 00003 import math 00004 from dataspeed_ulc_can import Speed 00005 from dataspeed_ulc_msgs.msg import UlcReport 00006 00007 00008 class SpeedSineWave(Speed): 00009 APPROACHING = 0 00010 TRACKING = 1 00011 00012 def __init__(self): 00013 rospy.init_node('speed_sine_wave') 00014 super(SpeedSineWave, self).__init__() 00015 00016 self.speed_meas = 0 00017 self.reached_target_stamp = -1 00018 self.state = self.APPROACHING 00019 rospy.Subscriber('/vehicle/ulc_report', UlcReport, self.recv_report) 00020 00021 def timer_callback(self, event): 00022 00023 if not self.enabled: 00024 self.t = 0 00025 self.state = self.APPROACHING 00026 return 00027 00028 if self.state == self.APPROACHING: 00029 self.ulc_cmd.linear_velocity = self.v1 00030 self.t = 0 00031 if abs(self.ulc_cmd.linear_velocity - self.speed_meas) < 0.4 and self.reached_target_stamp < 0: 00032 self.reached_target_stamp = event.current_real.to_sec() 00033 00034 # Wait 3 seconds before starting the sine wave input 00035 if self.reached_target_stamp > 0 and (event.current_real.to_sec() - self.reached_target_stamp) > 3: 00036 self.state = self.TRACKING 00037 self.reached_target_stamp = -1 00038 00039 elif self.state == self.TRACKING: 00040 amplitude = 0.5 * (self.v2 - self.v1) 00041 offset = 0.5 * (self.v2 + self.v1) 00042 self.ulc_cmd.linear_velocity = offset - amplitude * math.cos(2 * math.pi / self.period * self.t) 00043 self.t += 0.02 00044 00045 self.pub_ulc_cmd.publish(self.ulc_cmd) 00046 if self.v1 == 0 and self.v2 == 0: 00047 rospy.logwarn_throttle(1.0, 'both speed targets are zero') 00048 00049 def recv_report(self, msg): 00050 self.speed_meas = msg.speed_meas 00051 00052 00053 if __name__ == '__main__': 00054 try: 00055 node_instance = SpeedSineWave() 00056 rospy.spin() 00057 except rospy.ROSInterruptException: 00058 pass