def __init__(self, model_file):
self.model = pickle.load(open(model_file, 'rb'))
self.ego_velocity = 0
self.steering_output = 0.0
self.target_steering_angle = 0.0
self.current_steering_angle = 0.0
self.steering_accel = 0
self.steering_accel_2 = 0
self.net = torch.load('../../../data/out-model').eval()
self.reader = Reader()
self.mode = CONTROL_MODE.STRAIGHTENING
self.pi = PI(self.kP, self.kI, self.kF, -MAX_STEERING, MAX_STEERING)
self.p_pub = rospy.Publisher('spid_p', Float32, queue_size=1)
self.ff_pub = rospy.Publisher('spid_ff', Float32, queue_size=1)
self.i_pub = rospy.Publisher('spid_i', Float32, queue_size=1)
self.target_accel = rospy.Publisher('spid_targaccel', Float32, queue_size=1)
rospy.Subscriber("wheel_speed", Float32, self.on_speed)
rospy.Subscriber("/steering/wheel_angle/raw", Float32, self.on_wheel_angle)
rospy.Subscriber("/target_steering_angle", Float32, self.on_target_steering_angle)
rospy.Subscriber("/steering_accel", Float32, self.on_steer_accel)
rospy.Subscriber("/steering_accel_2", Float32, self.on_steer_accel_2)
self.steering_pub = rospy.Publisher('/steering_command', Float32, queue_size=1)
self.last_request_angle_time = None
self.controls_enabled = True
self.controls_enabled_sub = rospy.Subscriber('controls_enable', Bool, self.on_controls_enable)
rate = rospy.Rate(RATE)
while not rospy.is_shutdown():
self.pid_spin()
rate.sleep()
def __init__(self):
super().__init__('lateral_controler')
self.ego_accel = 0
self.ego_velocity = 0
self.brake_output = 0.0
self.throttle_output = 0.0
self.target_throttle = 0.0
self.target_brake = 0.0
self.mode = CONTROL_MODE.BRAKE
self.pi = PI(self.kP, self.kI, self.kF, -MAX_BRAKE, MAX_THROTTLE)
self.create_subscription(LongitudinalPlan, 'longitudinal_plan',
self.on_plan)
self.create_subscription(Float32, "debug_target_speed",
self.on_debug_target_speed)
self.plan_deviation_pub = self.create_publisher(
Float32, '/plan_deviation')
self.target_speed_pub = self.create_publisher(Float32,
'pid_target_speed')
self.target_acc = self.create_publisher(Float32, 'pid_target_accel')
self.p_pub = self.create_publisher(Float32, 'pid_p')
self.ff_pub = self.create_publisher(Float32, 'pid_ff')
self.i_pub = self.create_publisher(Float32, 'pid_i')
self.create_subscription(Float32, "wheel_speed", self.on_speed)
self.throttle_pub = self.create_publisher(Float32, '/throttle_command')
self.brake_pub = self.create_publisher(Float32, '/brake_command')
self.last_plan_time = None
self.controls_enabled = False
self.plan = None
self.acceleration_plan = None
self.velocity_plan = None
self.controls_enabled_sub = self.create_subscription(
Bool, 'controls_enable', self.on_controls_enable)
self.pid_timer = self.create_timer(1.0 / 50.0, self.pid_spin)
def __init__(self):
self.ego_velocity = 0
self.steering_angle = 0.0
self.cte = 0.0
self.curvature = 0.0
self.steering_output = 0.0
self.mode = CONTROL_MODE.STRAIGHTENING
self.pi = PI(self.kP, self.kI, self.kF, -MAX_STEERING_ANGLE,
MAX_STEERING_ANGLE)
self.p_pub = rospy.Publisher('spid_p', Float32, queue_size=1)
self.ff_pub = rospy.Publisher('spid_ff', Float32, queue_size=1)
self.i_pub = rospy.Publisher('spid_i', Float32, queue_size=1)
rospy.Subscriber("wheel_speed", Float32, self.on_speed)
rospy.Subscriber("/steering/wheel_angle/raw", Float32,
self.on_wheel_angle)
rospy.Subscriber("/cte", Float32, self.on_cte)
rospy.Subscriber("/curvature", Float32, self.on_curvature)
self.steering_pub = rospy.Publisher('/steering_angle_target',
Float32,
queue_size=1)
self.controls_enabled = True
self.controls_enabled_sub = rospy.Subscriber('controls_enable', Bool,
self.on_controls_enable)
rate = rospy.Rate(RATE)
while not rospy.is_shutdown():
self.pid_spin()
rate.sleep()
def __init__(self):
self.ego_accel = 0
self.ego_velocity = 0
self.brake_output = 0.0
self.throttle_output = 0.0
self.target_throttle = 0.0
self.target_brake = 0.0
self.mode = CONTROL_MODE.BRAKE
self.pi = PI(self.kP, self.kI, self.kF, -MAX_BRAKE, MAX_THROTTLE)
rospy.Subscriber('longitudinal_plan', LongitudinalPlan, self.on_plan)
rospy.Subscriber("debug_target_speed", Float32,
self.on_debug_target_speed)
self.plan_deviation_pub = rospy.Publisher('/plan_deviation',
Float32,
queue_size=1)
self.target_speed_pub = rospy.Publisher('pid_target_speed',
Float32,
queue_size=1)
self.target_acc = rospy.Publisher('pid_target_accel',
Float32,
queue_size=1)
self.p_pub = rospy.Publisher('pid_p', Float32, queue_size=1)
self.ff_pub = rospy.Publisher('pid_ff', Float32, queue_size=1)
self.i_pub = rospy.Publisher('pid_i', Float32, queue_size=1)
rospy.Subscriber("wheel_speed", Float32, self.on_speed)
self.throttle_pub = rospy.Publisher('/throttle_command',
Float32,
queue_size=1)
self.brake_pub = rospy.Publisher('/brake_command',
Float32,
queue_size=1)
self.last_plan_time = None
self.controls_enabled = False
self.plan = None
self.acceleration_plan = None
self.velocity_plan = None
self.controls_enabled_sub = rospy.Subscriber('controls_enable', Bool,
self.on_controls_enable)
rate = rospy.Rate(RATE)
while not rospy.is_shutdown():
self.pid_spin()
rate.sleep()