Die Migration der Bereiche "Docker Registry" und "Artifiacts" ist fast abgeschlossen. Die letzten Daten werden im Laufe des heutigen Abend (05.08.2021) noch vollständig hochgeladen. Das Anlegen neuer Images und Artifacts funktioniert bereits wieder.

Commit 485bd921 authored by Svetlana's avatar Svetlana
Browse files

2-lane driver controller in implemented Math

parent 15490e1f
......@@ -3,26 +3,38 @@ import dp.subcomponents.*;
component Mastercomponent {
ports
in Z(0:255)^{3, 210, 280} imageIn,
//in Z(0:255)^{3, 210, 280} imageIn,
in Q^{16} groundTruthAffordance,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
out Q(-1:1)^{3} commandsOut;
instance Dpnet dpnet;
//instance Dpnet dpnet;
instance DriverController driverController;
instance Denormalizer denorm;
//instance Denormalizer denorm;
//instance KFMastercomponent kfm;
instance SteeringBuffer steeringBuffer;
instance VectorToAffordance vecToAffordance;
instance LocalizationController locController;
connect imageIn -> dpnet.data;
connect dpnet.predictions -> denorm.normalizedPredictions;
//connect imageIn -> dpnet.data;
//connect dpnet.predictions -> denorm.normalizedPredictions;
//connect denorm.affordance -> kfm.affordanceIn;
//connect kfm.affordanceSmoothed -> driverController.affordanceIn;
connect denorm.affordance -> driverController.affordanceIn;
//connect denorm.affordance -> driverController.affordanceIn;
connect groundTruthAffordance -> vecToAffordance.affordanceIndicators;
connect vecToAffordance.affordance -> driverController.affordanceIn;
//connect vecToAffordance.affordance -> locController.affordanceIn;
connect steeringBuffer.outputBuffer -> driverController.steeringRecordIn;
connect speedIn -> driverController.speedIn;
connect steeringBuffer.timerLeftOut -> driverController.timerLeftIn;
connect steeringBuffer.timerRightOut -> driverController.timerRightIn;
connect steeringBuffer.laneChangeOut -> driverController.laneChangeIn;
connect driverController.commandsOut -> commandsOut;
connect driverController.steerCmd -> steeringBuffer.inSteerCmd;
}
\ No newline at end of file
connect driverController.timerLeftOut -> steeringBuffer.timerLeftIn;
connect driverController.timerRightOut -> steeringBuffer.timerRightIn;
connect driverController.laneChangeOut -> steeringBuffer.laneChangeIn;
}
......@@ -5,66 +5,177 @@ component DriverController {
in Q^{5} steeringRecordIn,
in Affordance affordanceIn,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
in Z timerLeftIn,
in Z timerRightIn,
in Z laneChangeIn,
out Q(-1:1)^{3} commandsOut, // [accelCmd, steerCmd, brakeCmd]
out Q steerCmd;
out Q steerCmd,
out Z timerLeftOut,
out Z timerRightOut,
out Z laneChangeOut;
implementation Math {
Q roadWidth = 8;
Q centerLine;
Q coeSteer = 1.0;
Z lane_change = laneChangeIn;
Q preML;
Q preMR;
Q desiredSpeed;
Q slowDown = 100; // Acceleration
Q distLLRecord = 30;
Q distRRRecord = 30;
Q preDistL = 60;
Q preDistR = 60;
Q steer_trend;
Z left_clear = 0;
Z right_clear = 0;
Z timer_set = 60;
Z left_timer = timerLeftIn;
Z right_timer = timerRightIn;
// Car following speed model: if a car is less than 20m ahead
if (affordanceIn.distMM < 20)
Q vMax = 20;
Q vC = 2.772;
Q vD = -0.693;
// optimal velocity car-following model
slowDown = vMax * (1-exp(-(vC/vMax) * affordanceIn.distMM - vD));
if (preDistL < 20 && affordanceIn.distLL < 20)
left_clear = 0;
left_timer = 0;
else
left_timer += 1;
end
if (preDistR < 20 && affordanceIn.distRR < 20)
right_clear = 0;
right_timer = 0;
else
right_timer += 1;
end
preDistL = affordanceIn.distLL;
preDistR = affordanceIn.distRR;
if (slowDown < 0)
slowDown = 0;
if (left_timer > timer_set)
left_timer = timer_set;
left_clear = 1;
end
if (right_timer > timer_set)
right_timer = timer_set;
right_clear = 1;
end
if (lane_change == 0 && affordanceIn.distMM < 15)
steer_trend = steeringRecordIn(1) + steeringRecordIn(2) + steeringRecordIn(3) + steeringRecordIn(4) + steeringRecordIn(5);
if (affordanceIn.toMarkingLL > -8 && left_clear == 1 && steer_trend >= 0)
lane_change = -2;
coeSteer = 6;
right_clear = 0;
right_timer = 0;
left_clear = 0;
left_timer = 0;
timer_set = 30;
elseif (affordanceIn.toMarkingRR < 8 && right_clear == 1 && steer_trend <= 0)
lane_change = 2;
coeSteer = 6;
right_clear = 0;
right_timer = 0;
left_clear = 0;
left_timer = 0;
timer_set = 30;
else
Q vMax = 20;
Q c = 2.772;
Q d = -0.693;
slowDown = vMax*(1-exp(-c/vMax * affordanceIn.distMM - d));
if slowDown < 0
slowDown = 0;
end
end
elseif (lane_change == 0 && affordanceIn.distMM >= 15)
steer_trend = steeringRecordIn(1) + steeringRecordIn(2) + steeringRecordIn(3) + steeringRecordIn(4) + steeringRecordIn(5);
if (affordanceIn.toMarkingLL < -8 && right_clear == 1 && steer_trend <= 0 && steer_trend > -0.2)
lane_change = 2;
coeSteer = 6;
right_clear = 0;
right_timer = 20;
end
end
if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
coeSteer = 1.5;
centerLine = (affordanceIn.toMarkingML + affordanceIn.toMarkingMR) / 2;
preML = affordanceIn.toMarkingML;
preMR = affordanceIn.toMarkingMR;
if (affordanceIn.toMarkingM < 1)
coeSteer = 0.4;
if (lane_change == 0)
if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
coeSteer = 1.5;
centerLine = (affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2;
preML = affordanceIn.toMarkingML;
preMR = affordanceIn.toMarkingMR;
if (affordanceIn.toMarkingM < 1)
coeSteer = 0.4;
end
else
if (-preML > preMR)
centerLine = (affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2;
else
centerLine = (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/2;
end
coeSteer = 0.3;
end
else
if (-preML > preMR)
centerLine = (affordanceIn.toMarkingL + affordanceIn.toMarkingM) / 2;
elseif (lane_change == -2)
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
centerLine = (affordanceIn.toMarkingLL+affordanceIn.toMarkingML)/2;
if (affordanceIn.toMarkingL > -5 && affordanceIn.toMarkingM < 1.5)
centerLine = (centerLine+(affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2)/2;
end
else
centerLine = (affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2;
coeSteer = 20;
lane_change = -1;
end
elseif (lane_change == -1)
if (affordanceIn.toMarkingL > -5 && affordanceIn.toMarkingM < 1.5)
centerLine = (affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2;
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
centerLine = (centerLine+(affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2)/2;
end
else
centerLine=(affordanceIn.toMarkingR + affordanceIn.toMarkingM) / 2;
centerLine = (affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2;
lane_change = 0;
end
elseif (lane_change == 2)
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
centerLine = (affordanceIn.toMarkingRR+affordanceIn.toMarkingMR)/2;
if (affordanceIn.toMarkingR < 5 && affordanceIn.toMarkingM < 1.5)
centerLine = (centerLine + (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/2)/2;
end
else
centerLine = (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/2;
coeSteer = 20;
lane_change = 1;
end
elseif (lane_change == 1)
if (affordanceIn.toMarkingR < 5 && affordanceIn.toMarkingM < 1.5)
centerLine = (affordanceIn.toMarkingR+affordanceIn.toMarkingM < 5.5)/2;
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
centerLine = (centerLine+(affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2)/2;
else
centerLine = (affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2;
lane_change = 0;
end
end
coeSteer = 0.3;
end
// steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052 / coeSteer;
steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052 / coeSteer;
if (coeSteer > 1 && steerCmd > 0.1) // reshape the steering control curve
steerCmd = steerCmd * (2.5 * steerCmd + 0.75);
if (lane_change == 0 && coeSteer > 1 && steerCmd > 0.1)
steerCmd = steerCmd * (2.5*steerCmd+0.75);
end
commandsOut(2) = steerCmd;
desiredSpeed = 20; // 20m/s = 72 kmh
if (affordanceIn.fast != 1)
desiredSpeed = 20 - abs(steeringRecordIn(1) + steeringRecordIn(2) + steeringRecordIn(3) + steeringRecordIn(4) + steeringRecordIn(5)) * 4.5;
desiredSpeed = 20 - abs(steer_trend) * 4.5;
end
if (desiredSpeed < 10)
desiredSpeed = 10;
end
if (desiredSpeed > slowDown)
desiredSpeed = slowDown;
end
......@@ -87,5 +198,9 @@ component DriverController {
commandsOut(3) = brakeCmd;
commandsOut(1) = 0; // accelCmd
end
timerLeftOut = left_timer;
timerRightOut = right_timer;
laneChangeOut = lane_change;
}
}
package dp.subcomponents;
component DriverController {
ports
in Q^{5} steeringRecordIn,
in Affordance affordanceIn,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
out Q(-1:1)^{3} commandsOut, // [accelCmd, steerCmd, brakeCmd]
out Q steerCmd;
implementation Math {
Q roadWidth = 8;
Q centerLine;
Q coeSteer = 1.0;
Q preML;
Q preMR;
Q desiredSpeed;
Q slowDown = 100; // Acceleration
// Car following speed model: if a car is less than 20m ahead
if (affordanceIn.distMM < 20)
Q vMax = 20;
Q vC = 2.772;
Q vD = -0.693;
// optimal velocity car-following model
slowDown = vMax * (1-exp(-(vC/vMax) * affordanceIn.distMM - vD));
if (slowDown < 0)
slowDown = 0;
end
end
if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
coeSteer = 1.5;
centerLine = (affordanceIn.toMarkingML + affordanceIn.toMarkingMR) / 2;
preML = affordanceIn.toMarkingML;
preMR = affordanceIn.toMarkingMR;
if (affordanceIn.toMarkingM < 1)
coeSteer = 0.4;
end
else
if (-preML > preMR)
centerLine = (affordanceIn.toMarkingL + affordanceIn.toMarkingM) / 2;
else
centerLine=(affordanceIn.toMarkingR + affordanceIn.toMarkingM) / 2;
end
coeSteer = 0.3;
end
// steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052 / coeSteer;
if (coeSteer > 1 && steerCmd > 0.1) // reshape the steering control curve
steerCmd = steerCmd * (2.5 * steerCmd + 0.75);
end
commandsOut(2) = steerCmd;
desiredSpeed = 20; // 20m/s = 72 kmh
if (affordanceIn.fast != 1)
desiredSpeed = 20 - abs(steeringRecordIn(1) + steeringRecordIn(2) + steeringRecordIn(3) + steeringRecordIn(4) + steeringRecordIn(5)) * 4.5;
end
if (desiredSpeed < 10)
desiredSpeed = 10;
end
if (desiredSpeed > slowDown)
desiredSpeed = slowDown;
end
// acceleration and brake
if (desiredSpeed >= speedIn)
Q accelCmd = 0.2*(desiredSpeed - speedIn + 1);
if (accelCmd > 1)
accelCmd = 1;
end
commandsOut(1) = accelCmd;
commandsOut(3) = 0; // brakeCmd
else
Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
if (brakeCmd > 1)
brakeCmd = 1;
end
commandsOut(3) = brakeCmd;
commandsOut(1) = 0; // accelCmd
end
}
}
......@@ -2,10 +2,30 @@ package dp.subcomponents;
component SteeringBuffer {
ports in Q inSteerCmd,
in Z timerLeftIn,
in Z timerRightIn,
in Z laneChangeIn,
out Z timerLeftOut,
out Z timerRightOut,
out Z laneChangeOut,
out Q^{5} outputBuffer;
implementation Math {
static Q^{5} buffer = [0; 0; 0; 0; 0];
static Q^{2} laneChangeBuffer = [0;0];
static Z^{2} timerLeftBuffer = [0; 0];
static Z^{2} timerRightBuffer = [0; 0];
timerLeftBuffer(1) = timerLeftBuffer(2);
timerRightBuffer(1) = timerRightBuffer(2);
timerLeftOut = timerLeftBuffer(1);
timerRightOut = timerRightBuffer(1);
timerLeftBuffer(2) = timerLeftIn;
timerRightBuffer(2) = timerRightIn;
laneChangeBuffer(1) = laneChangeBuffer(2);
laneChangeOut = laneChangeBuffer(1);
laneChangeBuffer(2) = laneChangeIn;
buffer(5) = buffer(4);
buffer(4) = buffer(3);
......@@ -19,4 +39,4 @@ component SteeringBuffer {
outputBuffer(4) = buffer(4);
outputBuffer(5) = buffer(5);
}
}
\ No newline at end of file
}
package dp.subcomponents;
import ChangeRange;
component VectorToAffordance {
ports in Q^{16} affordanceIndicators,
out Affordance affordance;
implementation Math {
affordance.fast = affordanceIndicators(1);
affordance.distL = affordanceIndicators(2);
affordance.distR = affordanceIndicators(3);
affordance.toMarkingL = affordanceIndicators(4);
affordance.toMarkingM = affordanceIndicators(5);
affordance.toMarkingR = affordanceIndicators(6);
affordance.distLL = affordanceIndicators(7);
affordance.distMM = affordanceIndicators(8);
affordance.distRR = affordanceIndicators(9);
affordance.toMarkingLL = affordanceIndicators(10);
affordance.toMarkingML = affordanceIndicators(11);
affordance.toMarkingMR = affordanceIndicators(12);
affordance.toMarkingRR = affordanceIndicators(13);
affordance.angle = affordanceIndicators(15);
}
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment