Source code for basic_test

#!/usr/bin/env python3
import rospy
from as_can_msgs.msg import SteeringControlEngage, VelocityTorqueSwitch, Velocity, SteeringWheelAngle, BrakePressure, AutonomousSystemState, Wheelspeed, Torque
from enum import Enum
import math
import time
import traceback


class AutonomousSystemStateEnum(Enum):
    STARTUP = 0
    OFF = 1
    READY = 2
    DRIVING = 3
    EMERGENCY = 4
    FINISHED = 5
    
    
class BasicTestEnum(Enum):
    EBS = 0
    ACTC_STEP = 1
    ACTC_SINUS = 2
    SERVICE_BRAKE = 3
    TORQUE = 4
    
    
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)
        self.velocity_torque_switch_pub = rospy.Publisher('/mission_machine/use_velocity',
                                                          VelocityTorqueSwitch, queue_size=10, latch=True)

        # get parameters 
        self.test = BasicTestEnum(rospy.get_param('/basic_tests/test'))  # which test should be executed
        self.velocity = rospy.get_param('/basic_tests/velocity')
        self.steering_frequency = rospy.get_param('/basic_tests/steering_frequency')
        self.max_angle = rospy.get_param('/basic_tests/max_angle')
        self.brake_pressure = rospy.get_param('/basic_tests/brake_pressure') 
        self.torque = rospy.get_param('/basic_tests/torque')     

        self.steering_wheel_offset = rospy.get_param('/vehicle/steering_wheel_angle_offset')
        
        self.actc_step_angle = 0
        self.current_brake_pressure = 0
        self.current_velocity = self.velocity
        # necessary for EBS test for easier managing of inverter
        self.ebs_target_velocity_reached = False

        # subscribe to AS state
        self.as_state = AutonomousSystemStateEnum(0)
        rospy.Subscriber('/can/per/as_state', AutonomousSystemState, self.as_state_callback)

        # velocity or torque
        self.velocity_pub = rospy.Publisher('/control/velocity/request', Velocity, queue_size=10, latch=True)
        self.torque_pub = rospy.Publisher('/control/torque/request', Torque, queue_size=10, latch=True)

        # steering wheel angle
        self.start_time = None
        self.steering_pub = rospy.Publisher('/control/steering_wheel_angle/request', SteeringWheelAngle, queue_size=10)
        
        # service brake
        self.service_brake_pub = rospy.Publisher('/control/service_brake/request', BrakePressure, queue_size=10, latch=True)
        
        # necessary for actc tests
        self.actual_velocity = 0
        self.wheel_radius = rospy.get_param('/vehicle/wheel_radius')
        self.gear_ratio = rospy.get_param('/vehicle/rear_gear_ratio')
        wheelspeed_sub = rospy.Subscriber('/can/log/wheelspeed/rear', Wheelspeed,
                                            self.rear_wheelspeed_callback, queue_size=10)
        
        rospy.on_shutdown(self.deactivate_steering)

        self.run()

    def as_state_callback(self, msg):
        self.as_state = AutonomousSystemStateEnum(msg.as_state)
    
    def rear_wheelspeed_callback(self, wheelspeed: Wheelspeed):
        self.actual_velocity = ((2 * math.pi * self.wheel_radius) / (60 * self.gear_ratio)) * \
        (wheelspeed.left + wheelspeed.right) / 2

    def run(self):
        # choose between torque and velocity
        use_velocity = self.test != BasicTestEnum.TORQUE
        self.publish_velocity_torque_switch(use_velocity)
        self.publish_steering_angle(0)
            
        # wait until driving is reached
        while not rospy.is_shutdown():
            if self.as_state == AutonomousSystemStateEnum.DRIVING:
                break
        
        # enable steering and choose between torque and velocity
        self.publish_steering_control_engage(steering_engage=True)
        if use_velocity:
            self.publish_velocity(0)
        else:
            self.publish_torque(0)
            
        # lower service brake pressure
        self.release_brakes(use_velocity)
        
        # run logic for test
        rospy.loginfo(f'[Basic Test] {self.test.name}')
        if self.test == BasicTestEnum.EBS:
            self.publish_velocity_torque_switch(use_velocity=False)
            self.publish_torque(0)
            run_method = self.ebs
        elif self.test == BasicTestEnum.ACTC_STEP:
            run_method = self.actc_step
        elif self.test == BasicTestEnum.ACTC_SINUS:
            run_method = self.actc_sinus
        elif self.test == BasicTestEnum.SERVICE_BRAKE:
            run_method = self.service_brake
        elif self.test == BasicTestEnum.TORQUE:
            run_method = self.torque_control

        rate = rospy.Rate(50)
        rospy.loginfo('[Basic Test] Go!')
        while not rospy.is_shutdown():
            run_method()
            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_velocity_torque_switch(self, use_velocity):
        velocity_torque_switch_msg = VelocityTorqueSwitch()
        velocity_torque_switch_msg.header.stamp = rospy.Time.now()
        velocity_torque_switch_msg.use_velocity = use_velocity
        self.velocity_torque_switch_pub.publish(velocity_torque_switch_msg)
                    
    def publish_velocity(self, velocity):
        velocity_msg = Velocity()
        velocity_msg.header.stamp = rospy.Time.now()
        velocity_msg.velocity = velocity
        self.velocity_pub.publish(velocity_msg)
        
    def publish_torque(self, torque):
        torque_msg = Torque()
        torque_msg.header.stamp = rospy.Time.now()
        torque_msg.torque = torque
        self.torque_pub.publish(torque_msg)
        
    def publish_service_brake(self, pressure=0):
        service_brake_msg = BrakePressure()
        service_brake_msg.header.stamp = rospy.Time.now()
        service_brake_msg.pressure = pressure
        self.service_brake_pub.publish(service_brake_msg)
            
    def release_brakes(self, use_velocity=True):
        if not rospy.is_shutdown():
            if use_velocity:
                self.publish_velocity(0)
            else:
                self.publish_torque(0)
            rospy.loginfo('[Basic Test] Release brakes.')
            self.publish_service_brake(0)
        
        rospy.sleep(5)
        
    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 ebs(self):
        if self.actual_velocity < 0.8 * self.velocity:
            self.publish_torque(self.torque)
        else:
            if not self.ebs_target_velocity_reached:
                self.publish_velocity_torque_switch(use_velocity=True)

            self.publish_velocity(self.velocity)

            self.ebs_target_velocity_reached = True

        self.publish_steering_angle(self.steering_wheel_offset)
        
    def actc_step_steering(self):
        if self.actual_velocity > 0.9 * self.velocity:
            self.actc_step_angle = self.max_angle
            
        self.publish_steering_angle(self.actc_step_angle)
        
    def actc_step(self):
        self.publish_velocity(self.velocity)
        self.actc_step_steering()
        
    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
        
        self.publish_steering_angle(angle)
        
    def actc_sinus(self):
        self.publish_velocity(self.velocity)
        self.actc_sinus_steering()
        
    def service_brake(self):
        if self.actual_velocity > 0.9 * self.velocity:
            self.publish_velocity_torque_switch(use_velocity=False)
            self.current_velocity = 0
            self.publish_torque(0)
            self.current_brake_pressure = self.brake_pressure
        elif self.actual_velocity < 0.1:
            self.current_velocity = self.velocity
            self.current_brake_pressure = 0
            self.publish_velocity_torque_switch(use_velocity=True)
        self.publish_service_brake(self.current_brake_pressure)
        self.publish_velocity(self.current_velocity)
        self.publish_steering_angle(0)

    def torque_control(self):
        self.publish_torque(self.torque)
        self.publish_steering_angle(0)
        

if __name__ == '__main__':
    rospy.init_node('basic_test', 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