Source code for sine_standing

#!/usr/bin/env python3
import rospy
from as_can_msgs.msg import SteeringControlEngage, SteeringWheelAngle
import math
import time
import traceback    
import numpy as np
    
    
class BasicTest: 
    def __init__(self):
        # necessary to simualate normal AS
        self.steering_control_engage_pub = rospy.Publisher('/mission_machine/steering_control_engage',
                                                           SteeringControlEngage, queue_size=10, latch=True)
        
        # get parameters 
        self.steering_frequency = rospy.get_param('/basic_tests/steering_frequency')
        self.max_angle = rospy.get_param('/basic_tests/max_angle')

        self.steering_wheel_offset = -0.9677 
        
        self.actc_step_angle = 0

        # steering wheel angle
        self.start_time = None
        self.steering_pub = rospy.Publisher('/control/steering_wheel_angle/request', SteeringWheelAngle, queue_size=10)
        
        rospy.on_shutdown(self.deactivate_steering)

        self.run()

    def run(self):
        # choose between torque and velocity
        self.publish_steering_angle(0)

        time.sleep(1/self.steering_frequency)
        # enable steering and choose between torque and velocity
        self.publish_steering_control_engage(steering_engage=True)
        
        rate = rospy.Rate(50)
        rospy.loginfo('[Basic Test] Go!')
        while not rospy.is_shutdown():
            self.actc_sinus_steering()
            rate.sleep()
        rospy.loginfo('[Basic Test] Stop!')
            
    def publish_steering_control_engage(self, steering_engage):
        steering_engage_msg = SteeringControlEngage()
        steering_engage_msg.header.stamp = rospy.Time.now()
        steering_engage_msg.engage = steering_engage
        self.steering_control_engage_pub.publish(steering_engage_msg)
        
    def publish_steering_angle(self, angle=0):
        steering_wheel_angle_request_msg = SteeringWheelAngle()
        steering_wheel_angle_request_msg.header.stamp = rospy.Time.now()
        steering_wheel_angle_request_msg.angle = angle
        self.steering_pub.publish(steering_wheel_angle_request_msg)
        
    def deactivate_steering(self):
        self.publish_steering_angle(0)
        self.publish_steering_control_engage(steering_engage=False)
        
    def actc_sinus_steering(self):
        now = rospy.Time.now()
        if self.start_time is None:
            self.start_time = now

        angle = math.sin(2 * math.pi * self.steering_frequency * (now - self.start_time).to_sec()) * self.max_angle

        #angle = np.clip(angle, -20, 20)
        self.publish_steering_angle(angle)
        #self.publish_steering_angle(self.max_angle)
       

if __name__ == '__main__':
    rospy.init_node('sine_standing', anonymous=True)
    try:
        BasicTest()
        rospy.spin()
    except Exception as e:
        if not isinstance(e, rospy.exceptions.ROSInterruptException):
            rospy.logfatal(traceback.format_exc())
            time.sleep(2)
        raise e