Commit c89f13f0 authored by Christoph Richter's avatar Christoph Richter
Browse files

Completely redesigned TrajectoryController

parent e60d5dff
......@@ -3,7 +3,7 @@ package de.monticore.lang.monticar.generator;
import de.monticore.lang.embeddedmontiarc.embeddedmontiarc._symboltable.ExpandedComponentInstanceSymbol;
import de.monticore.lang.monticar.generator.cpp.EMAMOpt2CPPSymbolTableHelper;
import de.monticore.lang.monticar.generator.cpp.GeneratorEMAMOpt2CPP;
import de.monticore.lang.monticar.generator.middleware.impls.GeneratorImpl;
import de.monticore.lang.monticar.generator.cpp.optimizationSolver.solver.Solver;
import de.monticore.lang.tagging._symboltable.TaggingResolver;
import org.junit.Test;
......@@ -13,22 +13,19 @@ import java.util.List;
import static org.junit.Assert.assertNotNull;
public class AutopilotMPCTest {
public class TrajectoryControllerTest extends BasicGenerationTest {
@Test
public void generateAutopilotMPCKindematicBycycle() throws IOException {
public void generateTrajectoryControllerMPCBicycle() throws IOException {
TaggingResolver symtab = EMAMOpt2CPPSymbolTableHelper.getInstance().createSymTabAndTaggingResolver("src/test/resources");
ExpandedComponentInstanceSymbol componentInstanceSymbol = symtab.<ExpandedComponentInstanceSymbol>resolve("de.rwth.armin.modeling.autopilot.autopilotMPC", ExpandedComponentInstanceSymbol.KIND).orElse(null);
ExpandedComponentInstanceSymbol componentInstanceSymbol = symtab.<ExpandedComponentInstanceSymbol>resolve("de.rwth.monticar.mpc.trajectoryControllerMPC", ExpandedComponentInstanceSymbol.KIND).orElse(null);
assertNotNull(componentInstanceSymbol);
GeneratorEMAMOpt2CMake generator = new GeneratorEMAMOpt2CMake();
for (GeneratorImpl gen : generator.getGeneratorImpls()) {
if (gen instanceof GeneratorEMAMOpt2CPP) {
GeneratorEMAMOpt2CPP emamoptGen = ((GeneratorEMAMOpt2CPP) gen);
emamoptGen.setGenerateAutopilotAdapter(true);
emamoptGen.setGenerateServerWrapper(false);
}
}
generator.setGenerationTargetPath("./target/generated-sources-cmake/AutopilotMPCKindematicBycycle/src/");
GeneratorEMAMOpt2CPP generator = new GeneratorEMAMOpt2CPP();
generator.setGenerateAutopilotAdapter(true);
generator.setGenerateServerWrapper(false);
generator.setGenerationTargetPath("./target/generated-sources-cpp/TrajectoryControllerMPC/src/");
generator.setPreferedSolver(Solver.Ipopt);
generator.getSolverOptions().put("String derivative_test", "none");
List<File> files = generator.generate(componentInstanceSymbol, symtab);
}
......
package de.rwth.armin.modeling.autopilot;
import de.rwth.armin.modeling.autopilot.common.*;
import de.rwth.armin.modeling.autopilot.behavior.*;
import de.rwth.armin.modeling.autopilot.mpc.kinematicbicycle.*;
component AutopilotMPC {
port
// length of simulation time frame
// e.g. when simulation frequency is 200 Hz
// this value will be equal to 5ms
in Q (0.0 s : 0.001 s : 1.0 s) timeIncrement,
// current velocity
in Q (0.0 m/s : 0.01 m/s : oo m/s) currentVelocity,
// current vehicle's position
in Q (-oo m : 0.01 m : oo m) x,
in Q (-oo m : 0.01 m : oo m) y,
// orientation angle (also sometimes called yaw angle) in radians
// it is measured from from the positive Y axis direction
// to the actual vehicle's direction counter-clockwise
// Example 1: vehicle has direction (-1.0; -1.0)
// compass = 0.25 * PI (positive)
// Example 2: vehicle has direction (1.0; 0.0) i.e. it is directed along the X axis
// compass = -0.5 * PI (negative)
in Q (-oo : 0.001 : oo) compass,
// current engine actuation
in Q (0.0 : 0.001 : 2.5) currentEngine,
// current steering actuation
// negative value: left turn
// positive value: right turn
in Q (-0.785 : 0.001 : 0.785) currentSteering,
// current brakes actuation
in Q (0.0 : 0.001 : 3.0) currentBrakes,
// planned trajectory (with look ahead 100-200m)
// represented by two arrays trajectory_x[] and trajectory_y[]
// which both have length trajectory_length
in Z (0 : 100) trajectory_length,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trajectory_x,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trajectory_y,
// output actuation commands
out Q (0.0 : 0.001 : 2.5) engine,
out Q (-0.785 : 0.001 : 0.785) steering,
out Q (0.0 : 0.001 : 3.0) brakes;
// trajectory planer
instance Compass2CurrentDirection c2cd;
instance BehaviorGeneration behaviorGeneration;
// mpc trajectory controller
instance KinematicBicycleMPCWrapper mpcTrajectoryController;
// connect trajectory generator
connect compass -> c2cd.compass;
connect x -> behaviorGeneration.currentPositionX;
connect y -> behaviorGeneration.currentPositionY;
connect trajectory_length -> behaviorGeneration.plannedTrajectoryLength;
connect trajectory_x -> behaviorGeneration.plannedTrajectoryX;
connect trajectory_y -> behaviorGeneration.plannedTrajectoryY;
connect c2cd.currentDirectionX -> behaviorGeneration.currentDirectionX;
connect c2cd.currentDirectionY -> behaviorGeneration.currentDirectionY;
// connect mpc trajectory controller
connect x -> mpcTrajectoryController.currentPositionX;
connect y -> mpcTrajectoryController.currentPositionY;
connect currentVelocity -> mpcTrajectoryController.currentVelocity;
connect compass -> mpcTrajectoryController.currentYawAngle;
connect trajectory_x -> mpcTrajectoryController.plannedTrajectoryX;
connect trajectory_y -> mpcTrajectoryController.plannedTrajectoryY;
connect behaviorGeneration.desiredDirectionX -> mpcTrajectoryController.desiredDirectionX;
connect behaviorGeneration.desiredDirectionY -> mpcTrajectoryController.desiredDirectionY;
connect behaviorGeneration.desiredVelocity -> mpcTrajectoryController.desiredVelocity;
// output
connect mpcTrajectoryController.engine -> engine;
connect mpcTrajectoryController.steering -> steering;
connect mpcTrajectoryController.brakes -> brakes;
}
package de.rwth.armin.modeling.autopilot.behavior;
component BehaviorGeneration {
port
in Q (-oo m : 0.01 m : oo m) currentPositionX,
in Q (-oo m : 0.01 m : oo m) currentPositionY,
in Q (-oo m : 0.01 m : oo m) currentDirectionX,
in Q (-oo m : 0.01 m : oo m) currentDirectionY,
in Z (0 : 100) plannedTrajectoryLength,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} plannedTrajectoryX,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} plannedTrajectoryY,
out Q (-oo m : 0.01 m : oo m) desiredDirectionX,
out Q (-oo m : 0.01 m : oo m) desiredDirectionY,
out Q (-oo m : 0.01 m : oo m) signedDistanceToTrajectory,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) desiredVelocity;
instance TrimPath trimPath;
instance CalculateMotionCommands calcMotionCmds;
connect currentPositionX -> trimPath.currentPositionX;
connect currentPositionY -> trimPath.currentPositionY;
connect plannedTrajectoryLength -> trimPath.plannedTrajectoryLength;
connect plannedTrajectoryX -> trimPath.plannedTrajectoryX;
connect plannedTrajectoryY -> trimPath.plannedTrajectoryY;
connect currentPositionX -> calcMotionCmds.currentPositionX;
connect currentPositionY -> calcMotionCmds.currentPositionY;
connect currentDirectionX -> calcMotionCmds.currentDirectionX;
connect currentDirectionY -> calcMotionCmds.currentDirectionY;
connect trimPath.trimmedTrajectoryLength -> calcMotionCmds.trimmedTrajectoryLength;
connect trimPath.trimmedTrajectoryX -> calcMotionCmds.trimmedTrajectoryX;
connect trimPath.trimmedTrajectoryY -> calcMotionCmds.trimmedTrajectoryY;
connect calcMotionCmds.desiredDirectionX -> desiredDirectionX;
connect calcMotionCmds.desiredDirectionY -> desiredDirectionY;
connect calcMotionCmds.signedDistanceToTrajectory -> signedDistanceToTrajectory;
connect calcMotionCmds.desiredVelocity -> desiredVelocity;
}
package de.rwth.armin.modeling.autopilot.behavior;
component CalculateMotionCommands {
port
in Q (-oo m : 0.01 m : oo m) currentPositionX,
in Q (-oo m : 0.01 m : oo m) currentPositionY,
in Q (-oo m : 0.01 m : oo m) currentDirectionX,
in Q (-oo m : 0.01 m : oo m) currentDirectionY,
in Z (0 : 100) trimmedTrajectoryLength,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryX,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryY,
out Q (-oo m : 0.01 m : oo m) desiredDirectionX,
out Q (-oo m : 0.01 m : oo m) desiredDirectionY,
out Q (-oo m : 0.01 m : oo m) signedDistanceToTrajectory,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) desiredVelocity;
instance IsDriveToFirstPosition isDriveToFirstPosition;
instance DriveToFirstPosition driveToFirstPosition;
instance Distance2Velocity d2v1;
instance FollowTrajectory followTrajectory;
instance Distance2Velocity d2v2;
instance Selector selector;
connect currentPositionX -> isDriveToFirstPosition.currentPositionX;
connect currentPositionY -> isDriveToFirstPosition.currentPositionY;
connect trimmedTrajectoryLength -> isDriveToFirstPosition.trimmedTrajectoryLength;
connect trimmedTrajectoryX -> isDriveToFirstPosition.trimmedTrajectoryX;
connect trimmedTrajectoryY -> isDriveToFirstPosition.trimmedTrajectoryY;
connect isDriveToFirstPosition.result -> driveToFirstPosition.isDriveToFirstPosition;
connect currentPositionX -> driveToFirstPosition.currentPositionX;
connect currentPositionY -> driveToFirstPosition.currentPositionY;
connect trimmedTrajectoryLength -> driveToFirstPosition.trimmedTrajectoryLength;
connect trimmedTrajectoryX -> driveToFirstPosition.trimmedTrajectoryX;
connect trimmedTrajectoryY -> driveToFirstPosition.trimmedTrajectoryY;
connect driveToFirstPosition.distance -> d2v1.distance;
connect driveToFirstPosition.minVelocity -> d2v1.minVelocity;
connect driveToFirstPosition.maxVelocity -> d2v1.maxVelocity;
connect isDriveToFirstPosition.result -> followTrajectory.isDriveToFirstPosition;
connect currentPositionX -> followTrajectory.currentPositionX;
connect currentPositionY -> followTrajectory.currentPositionY;
connect trimmedTrajectoryLength -> followTrajectory.trimmedTrajectoryLength;
connect trimmedTrajectoryX -> followTrajectory.trimmedTrajectoryX;
connect trimmedTrajectoryY -> followTrajectory.trimmedTrajectoryY;
connect followTrajectory.distance -> d2v2.distance;
connect followTrajectory.minVelocity -> d2v2.minVelocity;
connect followTrajectory.maxVelocity -> d2v2.maxVelocity;
connect isDriveToFirstPosition.result -> selector.isDriveToFirstPosition;
connect driveToFirstPosition.desiredDirectionX -> selector.desiredDirectionX1;
connect driveToFirstPosition.desiredDirectionY -> selector.desiredDirectionY1;
connect driveToFirstPosition.signedDistanceToTrajectory -> selector.signedDistanceToTrajectory1;
connect d2v1.velocity -> selector.desiredVelocity1;
connect followTrajectory.desiredDirectionX -> selector.desiredDirectionX2;
connect followTrajectory.desiredDirectionY -> selector.desiredDirectionY2;
connect followTrajectory.signedDistanceToTrajectory -> selector.signedDistanceToTrajectory2;
connect d2v2.velocity -> selector.desiredVelocity2;
connect selector.desiredDirectionX -> desiredDirectionX;
connect selector.desiredDirectionY -> desiredDirectionY;
connect selector.signedDistanceToTrajectory -> signedDistanceToTrajectory;
connect selector.desiredVelocity -> desiredVelocity;
}
package de.rwth.armin.modeling.autopilot.behavior;
import de.rwth.armin.modeling.autopilot.common.*;
component Distance2Velocity {
port
in Q (0.0 m : 0.01 m : oo m) distance,
in Q (0.0 m/s : 0.01 m/s : 13.0 m/s) minVelocity,
in Q (0.0 m/s : 0.01 m/s : 13.0 m/s) maxVelocity,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) velocity;
implementation Math {
// exponent
static Q D1 = 1.0 m;
static Q V1 = 1.0 m/s;
static Q D2 = 15.0 m;
static Q V2 = 7.0 m/s;
static Q COEF_K = log( (V1 / V2) ) / (D2 - D1);
Q v = 0.0;
if (distance < D1)
v = 0.0;
elseif (distance >= D2)
v = V2;
else
v = V1 * exp( -COEF_K * (distance - D1) );
end
if (v < minVelocity)
v = minVelocity;
elseif (v > maxVelocity)
v = maxVelocity;
end
velocity = v;
}
}
package de.rwth.armin.modeling.autopilot.behavior;
component DriveToFirstPosition {
port
in B isDriveToFirstPosition,
in Q (-oo m : 0.01 m : oo m) currentPositionX,
in Q (-oo m : 0.01 m : oo m) currentPositionY,
in Z (0 : 100) trimmedTrajectoryLength,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryX,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryY,
out Q (-oo m : 0.01 m : oo m) desiredDirectionX,
out Q (-oo m : 0.01 m : oo m) desiredDirectionY,
out Q (-oo m : 0.01 m : oo m) signedDistanceToTrajectory,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) minVelocity,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) maxVelocity,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) distance;
implementation Math {
desiredDirectionX = 0.0;
desiredDirectionY = 0.0;
signedDistanceToTrajectory = 0.0;
minVelocity = 0.0;
maxVelocity = 0.0;
distance = 0.0;
if (isDriveToFirstPosition) && (trimmedTrajectoryLength >= 1)
maxVelocity = 7.0;
desiredDirectionX = trimmedTrajectoryX(1,1) - currentPositionX;
desiredDirectionY = trimmedTrajectoryY(1,1) - currentPositionY;
Q sqr_dist = desiredDirectionX * desiredDirectionX + desiredDirectionY * desiredDirectionY;
distance = sqrt( sqr_dist );
if (trimmedTrajectoryLength >= 2)
minVelocity = 3.0;
end
end
}
}
package de.rwth.armin.modeling.autopilot.behavior;
component FollowTrajectory {
port
in B isDriveToFirstPosition,
in Q (-oo m : 0.01 m : oo m) currentPositionX,
in Q (-oo m : 0.01 m : oo m) currentPositionY,
in Z (0 : 100) trimmedTrajectoryLength,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryX,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryY,
out Q (-oo m : 0.01 m : oo m) desiredDirectionX,
out Q (-oo m : 0.01 m : oo m) desiredDirectionY,
out Q (-oo m : 0.01 m : oo m) signedDistanceToTrajectory,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) minVelocity,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) maxVelocity,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) distance;
implementation Math {
// change to params later
static Q BEZIER_COEF = 0.1;
static Q TURN_ANGLE_THRESHOLD = 10.0 * M_PI / 180.0; // 10 degrees
static Q EPSILON = 0.00001;
static Q REACHED_POSITION_THRESHOLD = 5.0;
desiredDirectionX = 0.0;
desiredDirectionY = 0.0;
signedDistanceToTrajectory = 0.0;
minVelocity = 0.0;
maxVelocity = 0.0;
distance = 0.0;
if (isDriveToFirstPosition)
Q doNothing;
elseif (trimmedTrajectoryLength >= 2)
maxVelocity = 7.0;
// SIGNED DISTANCE
Q p1x = trimmedTrajectoryX(1,1);
Q p1y = trimmedTrajectoryY(1,1);
Q p2x = trimmedTrajectoryX(1,2);
Q p2y = trimmedTrajectoryY(1,2);
Q v12x = p2x - p1x;
Q v12y = p2y - p1y;
Q v12_sqr_norm = v12x*v12x + v12y*v12y;
Q v12_norm = sqrt(v12_sqr_norm);
if (v12_norm > EPSILON)
// https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
signedDistanceToTrajectory = -(v12y*currentPositionX - v12x*currentPositionY + p2x*p1y - p1x*p2y) / v12_norm;
end
// DESIRED DIRECTION
Q nextX = currentPositionX;
Q nextY = currentPositionY;
if (trimmedTrajectoryLength >= 3)
Q interp_p1x = trimmedTrajectoryX(1,1);
Q interp_p1y = trimmedTrajectoryY(1,1);
Q interp_p2x = trimmedTrajectoryX(1,2);
Q interp_p2y = trimmedTrajectoryY(1,2);
Q interp_p3x = trimmedTrajectoryX(1,3);
Q interp_p3y = trimmedTrajectoryY(1,3);
// Bezier interpolation
Q t = BEZIER_COEF; // higher value => closer to p3
Q a = 1.0 - t;
Q b = t;
Q k1 = a * a;
Q k2 = 2 * a * b;
Q k3 = b * b;
nextX = k1 * interp_p1x + k2 * interp_p2x + k3 * interp_p3x;
nextY = k1 * interp_p1y + k2 * interp_p2y + k3 * interp_p3y;
Q dist_to_next_sqr = (nextX-currentPositionX)*(nextX-currentPositionX) + (nextY-currentPositionY)*(nextY-currentPositionY);
Q dist_to_next = sqrt(dist_to_next_sqr);
if (dist_to_next <= REACHED_POSITION_THRESHOLD)
// interpolated point is too close, another point needs to be found
Q is_go_on_1 = 1;
for j = 2:trimmedTrajectoryLength
if (is_go_on_1)
Q point_x = trimmedTrajectoryX(1,j);
Q point_y = trimmedTrajectoryY(1,j);
Q dist_to_point_sqr = (point_x-currentPositionX)*(point_x-currentPositionX) + (point_y-currentPositionY)*(point_y-currentPositionY);
Q dist_to_point = sqrt(dist_to_point_sqr);
if (dist_to_point > REACHED_POSITION_THRESHOLD)
nextX = point_x;
nextY = point_y;
is_go_on_1 = 0;
end
end
end
if (is_go_on_1)
// all points are too close
nextX = trimmedTrajectoryX(1,trimmedTrajectoryLength);
nextY = trimmedTrajectoryY(1,trimmedTrajectoryLength);
end
end
else
// only 2 points left
nextX = p2x;
nextY = p2y;
end
desiredDirectionX = nextX - currentPositionX;
desiredDirectionY = nextY - currentPositionY;
// DISTANCE
Q dist = sqrt( (p2x-p1x)*(p2x-p1x) + (p2y-p1y)*(p2y-p1y) );
if (trimmedTrajectoryLength >= 3)
minVelocity = 3.0;
Q is_go_on_2 = 1;
Q lastIndex = trimmedTrajectoryLength - 2;
for i = 1:lastIndex
if (is_go_on_2)
Q pt1x = trimmedTrajectoryX(1,i);
Q pt1y = trimmedTrajectoryY(1,i);
Q pt2x = trimmedTrajectoryX(1,i+1);
Q pt2y = trimmedTrajectoryY(1,i+1);
Q pt3x = trimmedTrajectoryX(1,i+2);
Q pt3y = trimmedTrajectoryY(1,i+2);
Q vect1x = pt2x - pt1x;
Q vect1y = pt2y - pt1y;
Q vect2x = pt3x - pt2x;
Q vect2y = pt3y - pt2y;
Q alpha = 0.0;
Q vect1_norm = sqrt( vect1x*vect1x + vect1y*vect1y );
Q vect2_norm = sqrt( vect2x*vect2x + vect2y*vect2y );
if ((vect1_norm > EPSILON) && (vect2_norm > EPSILON))
Q cos_alpha = (vect1x*vect2x + vect1y*vect2y) / vect1_norm / vect2_norm;
alpha = acos(cos_alpha);
end
if (alpha > TURN_ANGLE_THRESHOLD)
is_go_on_2 = 0;
else
dist += vect2_norm;
end
end
end
end
distance = dist;
end
}
}
package de.rwth.armin.modeling.autopilot.behavior;
component IsDriveToFirstPosition {
port
in Q (-oo m : 0.01 m : oo m) currentPositionX,
in Q (-oo m : 0.01 m : oo m) currentPositionY,
in Z (0 : 100) trimmedTrajectoryLength,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryX,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryY,
out B result;
implementation Math {
// maybe it can be moved to parameter later
Q TOO_FAR_FROM_TRAJECTORY_THRESHOLD = 3.0;
result = 1;
if trimmedTrajectoryLength > 0
Q dx = trimmedTrajectoryX(1,1) - currentPositionX;
Q dy = trimmedTrajectoryY(1,1) - currentPositionY;
Q sqr_dist = dx*dx + dy*dy;
Q dist_to_trajectory = sqrt( sqr_dist );
Q is_vehicle_went_off_trajectory = dist_to_trajectory > TOO_FAR_FROM_TRAJECTORY_THRESHOLD;
Q is_only_one_position_specified = trimmedTrajectoryLength <= 1;
result = is_vehicle_went_off_trajectory || is_only_one_position_specified;
end
}
}
package de.rwth.armin.modeling.autopilot.behavior;
component Selector {
port
in B isDriveToFirstPosition,
in Q (-oo m : 0.01 m : oo m) desiredDirectionX1,
in Q (-oo m : 0.01 m : oo m) desiredDirectionY1,
in Q (-oo m : 0.01 m : oo m) signedDistanceToTrajectory1,
in Q (0.0 m/s : 0.01 m/s : 13.0 m/s) desiredVelocity1,
in Q (-oo m : 0.01 m : oo m) desiredDirectionX2,
in Q (-oo m : 0.01 m : oo m) desiredDirectionY2,
in Q (-oo m : 0.01 m : oo m) signedDistanceToTrajectory2,
in Q (0.0 m/s : 0.01 m/s : 13.0 m/s) desiredVelocity2,
out Q (-oo m : 0.01 m : oo m) desiredDirectionX,
out Q (-oo m : 0.01 m : oo m) desiredDirectionY,
out Q (-oo m : 0.01 m : oo m) signedDistanceToTrajectory,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) desiredVelocity;
implementation Math {
if (isDriveToFirstPosition)
desiredDirectionX = desiredDirectionX1;
desiredDirectionY = desiredDirectionY1;
signedDistanceToTrajectory = signedDistanceToTrajectory1;
desiredVelocity = desiredVelocity1;
else
desiredDirectionX = desiredDirectionX2;
desiredDirectionY = desiredDirectionY2;
signedDistanceToTrajectory = signedDistanceToTrajectory2;
desiredVelocity = desiredVelocity2;
end
}
}
package de.rwth.armin.modeling.autopilot.behavior;
component Tests {
port in B in1, out B out1;
instance TrimPath trimPath;
}
package de.rwth.armin.modeling.autopilot.behavior;
import de.rwth.armin.modeling.autopilot.common.*;
// trims the planned path
// such that the first segment
// is the closest to the current vehicle's position
// and the first point is the projection
// of the current position onto the planned path
component TrimPath {
port
in Q (-oo m : 0.01 m : oo m) currentPositionX,
in Q (-oo m : 0.01 m : oo m) currentPositionY,
in Z (0 : 100) plannedTrajectoryLength,