#!/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
[docs]class AutonomousSystemStateEnum(Enum):
STARTUP = 0
OFF = 1
READY = 2
DRIVING = 3
EMERGENCY = 4
FINISHED = 5
[docs]class BasicTestEnum(Enum):
EBS = 0
ACTC_STEP = 1
ACTC_SINUS = 2
SERVICE_BRAKE = 3
TORQUE = 4
[docs]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()
[docs] def as_state_callback(self, msg):
self.as_state = AutonomousSystemStateEnum(msg.as_state)
[docs] 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
[docs] 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!')
[docs] 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)
[docs] 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)
[docs] 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)
[docs] 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)
[docs] 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)
[docs] 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)
[docs] 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)
[docs] def deactivate_steering(self):
self.publish_steering_angle(0)
self.publish_steering_control_engage(steering_engage=False)
[docs] 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)
[docs] 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)
[docs] def actc_step(self):
self.publish_velocity(self.velocity)
self.actc_step_steering()
[docs] 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)
[docs] def actc_sinus(self):
self.publish_velocity(self.velocity)
self.actc_sinus_steering()
[docs] 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)
[docs] 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