Commit e6473717 authored by Svetlana's avatar Svetlana

Refactorings

parent a1a22cb5
...@@ -15,14 +15,14 @@ component Mastercomponent { ...@@ -15,14 +15,14 @@ component Mastercomponent {
instance KFMastercomponent kfm; instance KFMastercomponent kfm;
instance SteeringBuffer steeringBuffer; instance SteeringBuffer steeringBuffer;
instance VectorToAffordance vecToAffordance; instance VectorToAffordance vecToAffordance;
instance LocalizationController locController; instance Localization locController;
instance AffordanceToVector affToVector; instance AffordanceToVector affToVector;
connect imageIn -> dpnet.data; connect imageIn -> dpnet.data;
connect dpnet.predictions -> denorm.normalizedPredictions; connect dpnet.predictions -> denorm.normalizedPredictions;
connect denorm.affordance -> kfm.affordanceIn; connect denorm.affordance -> kfm.affordanceIn;
connect kfm.affordanceSmoothed -> locController.affordanceIn, driverController.affordanceIn, affToVector.affordanceIn; connect kfm.affordanceSmoothed -> locController.affordanceIn, driverController.affordanceIn, affToVector.affordanceIn;
connect locController.lanesCount -> driverController.lanesCountIn; connect locController.numLanes -> driverController.lanesCountIn;
connect affToVector.affordanceOut -> predictedAffordanceOut; connect affToVector.affordanceOut -> predictedAffordanceOut;
//connect groundTruthAffordance -> vecToAffordance.affordanceIndicators; //connect groundTruthAffordance -> vecToAffordance.affordanceIndicators;
......
...@@ -4,7 +4,7 @@ component KFMastercomponent { ...@@ -4,7 +4,7 @@ component KFMastercomponent {
ports in Affordance affordanceIn, ports in Affordance affordanceIn,
out Affordance affordanceSmoothed; out Affordance affordanceSmoothed;
component SetSmoothedAffordance{ component AffordanceUpdater{
ports in Affordance affordanceIn, ports in Affordance affordanceIn,
in Q distLL, in Q distLL,
in Q distMM, in Q distMM,
...@@ -47,9 +47,9 @@ component KFMastercomponent { ...@@ -47,9 +47,9 @@ component KFMastercomponent {
instance KalmanStatsBuffer(affordanceIn.toMarkingML) statsML; instance KalmanStatsBuffer(affordanceIn.toMarkingML) statsML;
instance KalmanStatsBuffer(affordanceIn.toMarkingMR) statsMR; instance KalmanStatsBuffer(affordanceIn.toMarkingMR) statsMR;
instance SetSmoothedAffordance smoothedAffordance; instance AffordanceUpdater affordanceUpdater;
connect affordanceIn -> smoothedAffordance.affordanceIn; connect affordanceIn -> affordanceUpdater.affordanceIn;
connect stateTrans.out1 -> kfLL.stateTrans, kfMM.stateTrans, kfRR.stateTrans, kfML.stateTrans, kfMR.stateTrans; connect stateTrans.out1 -> kfLL.stateTrans, kfMM.stateTrans, kfRR.stateTrans, kfML.stateTrans, kfMR.stateTrans;
connect measM.out1 -> kfLL.measM, kfMM.measM, kfRR.measM, kfML.measM, kfMR.measM; connect measM.out1 -> kfLL.measM, kfMM.measM, kfRR.measM, kfML.measM, kfMR.measM;
connect procNoiseCov.out1 -> kfLL.procNoiseCov, kfMM.procNoiseCov, kfRR.procNoiseCov, kfML.procNoiseCov, kfMR.procNoiseCov; connect procNoiseCov.out1 -> kfLL.procNoiseCov, kfMM.procNoiseCov, kfRR.procNoiseCov, kfML.procNoiseCov, kfMR.procNoiseCov;
...@@ -60,35 +60,35 @@ component KFMastercomponent { ...@@ -60,35 +60,35 @@ component KFMastercomponent {
connect y_distLL.out1 -> kfLL.Y; connect y_distLL.out1 -> kfLL.Y;
connect kfLL.correctedX -> statsLL.X; connect kfLL.correctedX -> statsLL.X;
connect kfLL.correctedErrCov -> statsLL.errCov; connect kfLL.correctedErrCov -> statsLL.errCov;
connect kfLL.predictedMeasurement -> smoothedAffordance.distLL; connect kfLL.predictedMeasurement -> affordanceUpdater.distLL;
connect statsMM.outX -> kfMM.X; connect statsMM.outX -> kfMM.X;
connect statsMM.outErrCov -> kfMM.errCov; connect statsMM.outErrCov -> kfMM.errCov;
connect y_distLL.out1 -> kfMM.Y; connect y_distLL.out1 -> kfMM.Y;
connect kfMM.correctedX -> statsMM.X; connect kfMM.correctedX -> statsMM.X;
connect kfMM.correctedErrCov -> statsMM.errCov; connect kfMM.correctedErrCov -> statsMM.errCov;
connect kfMM.predictedMeasurement -> smoothedAffordance.distMM; connect kfMM.predictedMeasurement -> affordanceUpdater.distMM;
connect statsRR.outX -> kfRR.X; connect statsRR.outX -> kfRR.X;
connect statsRR.outErrCov -> kfRR.errCov; connect statsRR.outErrCov -> kfRR.errCov;
connect y_distRR.out1 -> kfRR.Y; connect y_distRR.out1 -> kfRR.Y;
connect kfRR.correctedX -> statsRR.X; connect kfRR.correctedX -> statsRR.X;
connect kfRR.correctedErrCov -> statsRR.errCov; connect kfRR.correctedErrCov -> statsRR.errCov;
connect kfRR.predictedMeasurement -> smoothedAffordance.distRR; connect kfRR.predictedMeasurement -> affordanceUpdater.distRR;
connect statsML.outX -> kfML.X; connect statsML.outX -> kfML.X;
connect statsML.outErrCov -> kfML.errCov; connect statsML.outErrCov -> kfML.errCov;
connect y_toMarkingML.out1 -> kfML.Y; connect y_toMarkingML.out1 -> kfML.Y;
connect kfML.correctedX -> statsML.X; connect kfML.correctedX -> statsML.X;
connect kfML.correctedErrCov -> statsML.errCov; connect kfML.correctedErrCov -> statsML.errCov;
connect kfML.predictedMeasurement -> smoothedAffordance.toMarkingML; connect kfML.predictedMeasurement -> affordanceUpdater.toMarkingML;
connect statsMR.outX -> kfMR.X; connect statsMR.outX -> kfMR.X;
connect statsMR.outErrCov -> kfMR.errCov; connect statsMR.outErrCov -> kfMR.errCov;
connect y_toMarkingMR.out1 -> kfMR.Y; connect y_toMarkingMR.out1 -> kfMR.Y;
connect kfMR.correctedX -> statsMR.X; connect kfMR.correctedX -> statsMR.X;
connect kfMR.correctedErrCov -> statsMR.errCov; connect kfMR.correctedErrCov -> statsMR.errCov;
connect kfMR.predictedMeasurement -> smoothedAffordance.toMarkingMR; connect kfMR.predictedMeasurement -> affordanceUpdater.toMarkingMR;
connect smoothedAffordance.affordanceOut -> affordanceSmoothed; connect affordanceUpdater.affordanceOut -> affordanceSmoothed;
} }
package dp.subcomponents; package dp.subcomponents;
component LocalizationController { component Localization {
ports ports
in Affordance affordanceIn, in Affordance affordanceIn,
out B onLane, out B onLane,
out B onMarking, out B onMarking,
out Z lanesCount, out Z numLanes,
out Lane lane; out Lane lane;
implementation Math { implementation Math {
...@@ -17,15 +17,15 @@ component LocalizationController { ...@@ -17,15 +17,15 @@ component LocalizationController {
onLane = true; onLane = true;
onMarking = false; onMarking = false;
if (affordanceIn.toMarkingLL <= -7.5 && affordanceIn.toMarkingRR >= 7.5) if (affordanceIn.toMarkingLL <= -7.5 && affordanceIn.toMarkingRR >= 7.5)
lanesCount = 1; numLanes = 1;
lane = CENTER_LANE; lane = CENTER_LANE;
elseif (affordanceIn.toMarkingLL > -7.5 && affordanceIn.toMarkingRR < 7.5) elseif (affordanceIn.toMarkingLL > -7.5 && affordanceIn.toMarkingRR < 7.5)
lanesCount = 3; numLanes = 3;
lane = CENTER_LANE; lane = CENTER_LANE;
elseif (affordanceIn.toMarkingLL > -7.5 || affordanceIn.toMarkingRR < 7.5) elseif (affordanceIn.toMarkingLL > -7.5 || affordanceIn.toMarkingRR < 7.5)
lanesCount = 2; numLanes = 2;
if (affordanceIn.toMarkingRR >=7.5) if (affordanceIn.toMarkingRR >=7.5)
lane = RIGHT_LANE; lane = RIGHT_LANE;
end end
...@@ -34,7 +34,7 @@ component LocalizationController { ...@@ -34,7 +34,7 @@ component LocalizationController {
end end
else else
lanesCount = 0; numLanes = 0;
onLane = false; onLane = false;
lane = OUTSIDE_LANE; lane = OUTSIDE_LANE;
end end
...@@ -43,12 +43,12 @@ component LocalizationController { ...@@ -43,12 +43,12 @@ component LocalizationController {
onMarking = true; onMarking = true;
// Car on central marking // Car on central marking
if (affordanceIn.toMarkingR < 6 && affordanceIn.toMarkingL > -6) if (affordanceIn.toMarkingR < 6 && affordanceIn.toMarkingL > -6)
lanesCount = 2; numLanes = 2;
lane = CENTER_LANE; lane = CENTER_LANE;
// When on Outer lanes consider the road only 1 lane. // When on Outer lanes consider the road only 1 lane.
else else
lanesCount = 1; numLanes = 1;
if (affordanceIn.toMarkingL < -6) if (affordanceIn.toMarkingL < -6)
lane = LEFT_LANE; lane = LEFT_LANE;
elseif (affordanceIn.toMarkingR > 6) elseif (affordanceIn.toMarkingR > 6)
......
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