sinetest.py

#!/usr/bin/python
from __future__ import absolute_import, print_function, division
import rospy
import argparse
from geometry_msgs.msg import Twist
import math

parser = argparse.ArgumentParser(description='Step input followed by ramp')
parser.add_argument('sin_period', type=float, help='Duration to do sine wave')
parser.add_argument('number_of_periods', type=float, help='number of full periods')
parser.add_argument('--frequency', '-hz', type=float, default=30, help='Control publish frequency')
parser.add_argument('--delay', '-d', type=float, default=5.00, help='Delay between actions in seconds, defaults to 5.0s')
parser.add_argument('--max', '-M', type=float, default=4.0, help='Max commanded velocity')
parser.add_argument('--min', '-m', type=float, default=-2.0, help='min commanded velocity')

args = parser.parse_args()
publisher = rospy.Publisher("cmd_vel/default", Twist, queue_size=1)

rospy.init_node("control_calibration_script_test_only")

DELAY = args.delay
MAX = args.max
MIN = args.min
HZ = args.frequency


def get_twist_msg(velocity):
    msg = Twist()
    msg.linear.x = velocity
    return msg


def perform(function, duration):
    start = rospy.Time.now().to_sec()
    rate = rospy.Rate(HZ)
    while not rospy.is_shutdown() and (rospy.Time.now().to_sec() - start) < duration:
        t = rospy.Time.now().to_sec() - start
        control_signal = function(t)
        publisher.publish(get_twist_msg(control_signal))
        rate.sleep()
    if rospy.is_shutdown():
        exit()


def hold_zero(duration):
    perform(lambda t: 0, duration)

# Zero
print("Delaying with zeros for {} seconds".format(1))
hold_zero(1)

# Ramp up
def sin(t):
    frequency = 2 * math.pi / args.sin_period
    start_zero_time = (2*math.pi+math.asin((MAX+MIN)/(MAX-MIN)) + math.pi) / frequency
    return (math.sin((t+start_zero_time) * frequency) + 1) * ((MAX-MIN) / 2) + MIN
print("Sine wave from {} to {} with a period {}, {} times.".format(MIN, MAX, args.sin_period, args.number_of_periods))
perform(sin, args.sin_period * args.number_of_periods)

# Zero
print("Sin complete. Delaying with zeros for {} seconds before exiting.".format(1))
hold_zero(1)