#!/usr/bin/env python3
"""
@author: Oliver
STATUS: in progress
"""
import rospy
import os
import numpy as np
from utilities.msg import VehiclePose, TrajectoryPoints
from as_can_msgs.msg import SteeringWheelAngle, Torque, BrakePressure
[docs]class MPC_Simulation():
def __init__(self):
# Initialize subscriber
rospy.Subscriber('/control/steering_wheel_angle/request', SteeringWheelAngle, self.steering_callback)
rospy.Subscriber('/control/torque/request', Torque, self.velocity_callback)
rospy.Subscriber('control/service_brake/request', BrakePressure, self.brake_callback)
# Initialize publisher
self.TrajectoryPublisher = rospy.Publisher('/motion_planning/trajectory', TrajectoryPoints, queue_size=10)
self.VehiclePosePublisher = rospy.Publisher('/slam/vehicle_pose', VehiclePose, queue_size=10)
[docs] def straight_trajectory(self):
"""
straight line with velocity profile
"""
x = np.arange(0, 100, 0.1) # x-array
y = np.zeros(np.size(x)) # y-array
v = np.zeros(int(np.size(x) / 5.0)) + 10.0 # velocity
v = np.append(v, np.zeros(int(np.size(x) / 5.0)) + 5)
v = np.append(v, np.zeros(int(np.size(x) / 5.0)) + 5)
v = np.append(v, np.zeros(int(np.size(x) / 5.0)) + 5)
v = np.append(v, np.zeros(int(np.size(x) / 5.0)) + 2)
trajectory = np.array([x, y, v])
return trajectory
[docs] def getRaceTrack(self):
"""
Load trajectory from CSV
"""
table = np.genfromtxt(f'{os.path.abspath(os.path.dirname(__file__))}/viktor8_trajectory_vmax15.csv')
x = np.transpose(table[:, 0])
y = np.transpose(table[:, 1])
v = np.transpose(table[:, 2])
trajectory = np.array([x, y, v])
return trajectory
[docs] def steering_callback(self, data):
rospy.loginfo(f'Steering Angle {data.angle/np.pi*180.0} degree')
[docs] def velocity_callback(self, data):
rospy.loginfo(f'Motor Torque: {data.torque} Nm')
[docs] def brake_callback(self, data):
rospy.loginfo(f'Brake Pressure {data.pressure} Bar')
[docs] def publish_messages(self):
trajectory = self.straight_trajectory()
x1 = trajectory[0, 0]
x2 = trajectory[0, 1]
y1 = trajectory[1, 0]
y2 = trajectory[1, 1]
psi1 = np.arctan2(y2 - y1, x2 - x1)
v1 = trajectory[2, 0]
# Publish trajectory
msg = TrajectoryPoints()
msg.header.stamp = rospy.Time.now()
msg.x = trajectory[0, :]
msg.y = trajectory[1, :]
msg.v = trajectory[2, :]
self.TrajectoryPublisher.publish(msg)
# Publish vehicle pose
msg = VehiclePose()
msg.header.stamp = rospy.Time.now()
msg.x = float(x1)
msg.y = float(y1)
msg.v_x = float(v1)
msg.psi = float(psi1)
self.VehiclePosePublisher.publish(msg)
if __name__ == '__main__':
rospy.init_node('control_simulation', anonymous=False)
simulation = MPC_Simulation()
rate = rospy.Rate(1)
while not rospy.is_shutdown():
simulation.publish_messages()
rate.sleep()