Aufgrund einer Wartung wird GitLab am 18.01. zwischen 8:00 und 9:00 Uhr kurzzeitig nicht zur Verfügung stehen. / Due to maintenance, GitLab will be temporarily unavailable on 18.01. between 8:00 and 9:00 am.

Commit 15e8b9e6 authored by Jean Meurice's avatar Jean Meurice
Browse files

Automated Json, new eecomponent builders

parent 4c76a70f
{
"name": "UnnamedCar",
"properties": {
"body": {
"mass": 1642.0,
"length": 4.971,
"width": 1.87,
"height": 1.383,
"center_of_gravity_height": 0.34575
},
"wheels": {
"diameter": 0.6612,
"width": 0.255,
"front_wheel_axis_offset": 1.0,
"max_turning_angle": 30.0,
"front_track_width": 1.57988,
"back_track_dist": 1.60274,
"wheelbase": 2.812,
"front_wheels_depth": 1.406,
"back_wheels_depth": 1.406
}
},
"powertrain": {
"type": "electrical",
"traction": "rear",
"braking": "rear",
"max_braking_force": 5000.0,
"motor": {
"motor_efficiency": 0.7,
"regen_efficiency": 0.5,
"motor_peek_torque": 450.0
},
"battery": {
"type": "infinite",
"capacity": 1.8E8,
"critical_charge": 10.0
},
"transmission_ratio": 9.0
},
"physics": {
"type": "rigidbody"
},
"components": [
{
"type": "actuator",
"name": "SteeringActuator",
"priority": {},
"buses": [
"DefaultBus"
],
"physical_value_name": "steering",
"min": -30.0,
"max": 30.0,
"change_rate": 60.0,
"sensor": {
"type": "sensor",
"name": "UnnamedComponent",
"priority": {},
"buses": [
"DefaultBus"
],
"update_interval": [
0,
100000000
],
"read_time": [
0,
1000000
],
"send_only_changed": false
}
},
{
"type": "actuator",
"name": "BrakingActuator",
"priority": {},
"buses": [
"DefaultBus"
],
"physical_value_name": "braking",
"min": 0.0,
"max": 1.0,
"change_rate": 10.0,
"sensor": {
"type": "sensor",
"name": "UnnamedComponent",
"priority": {},
"buses": [
"DefaultBus"
],
"update_interval": [
0,
100000000
],
"read_time": [
0,
1000000
],
"send_only_changed": false
}
},
{
"type": "actuator",
"name": "GasActuator",
"priority": {},
"buses": [
"DefaultBus"
],
"physical_value_name": "gas",
"min": -0.5,
"max": 1.0,
"change_rate": "Infinity",
"sensor": {
"type": "sensor",
"name": "UnnamedComponent",
"priority": {},
"buses": [
"DefaultBus"
],
"update_interval": [
0,
100000000
],
"read_time": [
0,
1000000
],
"send_only_changed": false
}
},
{
"type": "sensor",
"name": "TrueVelocitySensor",
"priority": {},
"buses": [
"DefaultBus"
],
"physical_value_name": "true_velocity",
"update_interval": [
0,
100000000
],
"read_time": [
0,
10000000
],
"send_only_changed": false
},
{
"type": "sensor",
"name": "TruePositionSensor",
"priority": {},
"buses": [
"DefaultBus"
],
"physical_value_name": "true_position",
"update_interval": [
0,
100000000
],
"read_time": [
0,
10000000
],
"send_only_changed": false
},
{
"type": "sensor",
"name": "TrueCompassSensor",
"priority": {},
"buses": [
"DefaultBus"
],
"physical_value_name": "true_compass",
"update_interval": [
0,
100000000
],
"read_time": [
0,
10000000
],
"send_only_changed": false
},
{
"type": "navigation",
"name": "Navigation",
"priority": {},
"buses": [
"DefaultBus"
]
},
{
"type": "constant_bus",
"name": "DefaultBus",
"priority": {},
"buses": [
"DefaultBus"
],
"mode": "instant",
"rate": 0.0
},
{
"type": "java_autopilot",
"name": "TestAutopilot",
"priority": {},
"buses": [
"DefaultBus"
],
"compute_time": [
0,
0
]
}
]
}
\ No newline at end of file
......@@ -23,23 +23,23 @@ import de.rwth.montisim.simulation.vehicle.physicalvalues.TrueVelocity;
import de.rwth.montisim.simulation.vehicle.powertrain.PowerTrainProperties;
public class JavaAutopilot extends EEComponent {
final JavaAutopilotProperties properties;
transient final JavaAutopilotProperties properties;
public static final double MAX_DEVIATION = 3; // max allowed deviation from the trajectory "corners" (in meters)
public static final double ORTHO_DIST = MAX_DEVIATION/(Math.sqrt(2)-1); // max allowed deviation from the trajectory "corners" (in meters)
MessageInformation velocityMsg;
MessageInformation positionMsg;
MessageInformation compassMsg;
public static final double ORTHO_DIST = MAX_DEVIATION / (Math.sqrt(2) - 1); // max allowed deviation from the
// trajectory "corners" (in meters)
MessageInformation trajXMsg;
MessageInformation trajYMsg;
transient MessageInformation velocityMsg;
transient MessageInformation positionMsg;
transient MessageInformation compassMsg;
transient MessageInformation trajXMsg;
transient MessageInformation trajYMsg;
transient MessageInformation steeringMsg;
transient MessageInformation accelMsg;
transient MessageInformation brakeMsg;
MessageInformation steeringMsg;
MessageInformation accelMsg;
MessageInformation brakeMsg;
public double currentVelocity = 0;
public Vec2 currentPosition = null;
public double currentCompass = Double.NaN;
......@@ -47,18 +47,18 @@ public class JavaAutopilot extends EEComponent {
double newTrajX[] = null;
public double trajX[] = null;
public double trajY[] = null;
Instant lastTime = null;
final PID speedPid;
final PID turnPid;
transient final PID speedPid;
transient final PID turnPid;
public JavaAutopilot(JavaAutopilotProperties properties) {
super(properties);
this.properties = properties;
this.speedPid = new PID(1,0,0.2);
this.turnPid = new PID(1,0,0.2);
this.speedPid = new PID(1, 0, 0.2);
this.turnPid = new PID(1, 0, 0.2);
}
@Override
protected void init() throws EEMessageTypeException {
this.velocityMsg = addInput(TrueVelocity.VALUE_NAME, TrueVelocity.TYPE);
......@@ -68,9 +68,10 @@ public class JavaAutopilot extends EEComponent {
this.trajXMsg = addInput(Navigation.TRAJECTORY_X_MSG, Navigation.TRAJECTORY_X_TYPE);
this.trajYMsg = addInput(Navigation.TRAJECTORY_Y_MSG, Navigation.TRAJECTORY_Y_TYPE);
this.steeringMsg = addOutput(Actuator.SETTER_PREFIX+PowerTrainProperties.STEERING_VALUE_NAME, DataType.DOUBLE);
this.accelMsg = addOutput(Actuator.SETTER_PREFIX+PowerTrainProperties.GAS_VALUE_NAME, DataType.DOUBLE);
this.brakeMsg = addOutput(Actuator.SETTER_PREFIX+PowerTrainProperties.BRAKING_VALUE_NAME, DataType.DOUBLE);
this.steeringMsg = addOutput(Actuator.SETTER_PREFIX + PowerTrainProperties.STEERING_VALUE_NAME,
DataType.DOUBLE);
this.accelMsg = addOutput(Actuator.SETTER_PREFIX + PowerTrainProperties.GAS_VALUE_NAME, DataType.DOUBLE);
this.brakeMsg = addOutput(Actuator.SETTER_PREFIX + PowerTrainProperties.BRAKING_VALUE_NAME, DataType.DOUBLE);
}
@Override
......@@ -84,10 +85,10 @@ public class JavaAutopilot extends EEComponent {
compute(msgRecvEvent.getEventTime());
} else if (msg.msgId == compassMsg.messageId) {
currentCompass = (Double) msg.message;
} else if (msg.msgId == trajXMsg.messageId){
} else if (msg.msgId == trajXMsg.messageId) {
// Assumes the x positions array of a new trajectory always arrives first
newTrajX = (double[]) msg.message;
} else if (msg.msgId == trajYMsg.messageId){
} else if (msg.msgId == trajYMsg.messageId) {
trajY = (double[]) msg.message;
trajX = newTrajX;
}
......@@ -107,9 +108,9 @@ public class JavaAutopilot extends EEComponent {
double dist;
double projDistToEnd;
void initFromTraj(int index){
void initFromTraj(int index) {
getTrajPoint(index, posStart);
getTrajPoint(index+1, posEnd);
getTrajPoint(index + 1, posEnd);
init();
}
......@@ -117,10 +118,10 @@ public class JavaAutopilot extends EEComponent {
void init() {
IPM.subtractTo(dir, posEnd, posStart);
length = dir.magnitude();
if (length > 0.001){
IPM.multiply(dir, 1/length);
if (length > 0.001) {
IPM.multiply(dir, 1 / length);
} else {
dir.set(Double.NaN,Double.NaN);
dir.set(Double.NaN, Double.NaN);
}
normal.set(-dir.y, dir.x);
......@@ -145,21 +146,22 @@ public class JavaAutopilot extends EEComponent {
int target = 0;
void compute(Instant startTime) {
if (currentPosition == null || Double.isNaN(currentCompass)) return;
if (currentPosition == null || Double.isNaN(currentCompass))
return;
double carAngle = currentCompass * Geometry.DEG_TO_RAD;
Instant sendTime = startTime.plus(properties.computeTime);
Instant sendTime = startTime.plus(properties.compute_time);
int index = getNearestSegment(currentPosition);
if (index < 0){
if (index < 0) {
// No trajectory -> Stay in place
sendMessage(sendTime, accelMsg, 0.0);
sendMessage(sendTime, brakeMsg, 1.0);
return;
}
carDir.set(Math.cos(carAngle), Math.sin(carAngle));
if (trajX.length == 1 || index + 1 >= trajX.length){
if (trajX.length == 1 || index + 1 >= trajX.length) {
// Only one point -> orient towards it
// If "behind" -> just stop
// Orient and try to stop at position
......@@ -169,7 +171,7 @@ public class JavaAutopilot extends EEComponent {
double dt = getDeltaTime(startTime);
SegmentPos targetSeg = findTargetSegment(index);
// Now that we have a target segment, we follow it
double turnOutput = 0;
double speedOutput = 0;
......@@ -179,56 +181,56 @@ public class JavaAutopilot extends EEComponent {
double segmentAngle = Math.acos(targetSeg.dir.x) * Math.signum(targetSeg.dir.y);
// Define TURNING to align with target (depending on distance, orientation & speed)
// Define TURNING to align with target (depending on distance, orientation &
// speed)
// Target angle (relative to segment)
double margin = 0.2;
double targetAngleRel = targetSeg.dist < margin ?
0 :
targetSeg.dist < ORTHO_DIST ?
Math.acos(1+(margin-targetSeg.dist)/ORTHO_DIST) :
Math.PI*0.5;
double targetAngleRel = targetSeg.dist < margin ? 0
: targetSeg.dist < ORTHO_DIST ? Math.acos(1 + (margin - targetSeg.dist) / ORTHO_DIST) : Math.PI * 0.5;
// If targetDir attained: check if "tangent turn" is possible
// targetDir attained = car pointing between orthogonal to segment & targetDir (CHECK symmetric +/- versions)
// targetDir attained = car pointing between orthogonal to segment & targetDir
// (CHECK symmetric +/- versions)
if (carTowardsSeg && carTowardsEnd && Math.abs(carAngle - segmentAngle) < targetAngleRel) {
if (mode == 0) {
//System.out.println("Tangent turn");
// System.out.println("Tangent turn");
mode = 1;
}
// Find the radius of a circle tangent to the segment and the car position
double junctionAngle = Math.acos(IPM.dot(targetSeg.dir, carDir));
double radius = Math.tan( (Math.PI - junctionAngle)*0.5) * targetSeg.dist / IPM.dot(carDir, targetSeg.normal);
double radius = Math.tan((Math.PI - junctionAngle) * 0.5) * targetSeg.dist
/ IPM.dot(carDir, targetSeg.normal);
turnOutput = 452/radius * Math.signum(-targetSeg.orthoPos);
turnOutput = 452 / radius * Math.signum(-targetSeg.orthoPos);
} else {
if (mode == 1) {
//System.out.println("Follow angle");
// System.out.println("Follow angle");
mode = 0;
}
double targetAngle = segmentAngle;
targetAngle += targetAngleRel * Math.signum(-targetSeg.orthoPos);
targetDir.set(Math.cos(targetAngleRel), Math.sin(targetAngleRel));
while (targetAngle > carAngle+Math.PI) targetAngle -= 2*Math.PI;
while (targetAngle < carAngle-Math.PI) targetAngle += 2*Math.PI;
while (targetAngle > carAngle + Math.PI)
targetAngle -= 2 * Math.PI;
while (targetAngle < carAngle - Math.PI)
targetAngle += 2 * Math.PI;
turnOutput = turnPid.compute(dt, carAngle, targetAngle);
turnOutput *= Geometry.RAD_TO_DEG;
}
// Eval required speed -> in order to arrive at next turn with desired max speed or arrive at speed 0 at end
// Eval required speed -> in order to arrive at next turn with desired max speed
// or arrive at speed 0 at end
// Respect max speed
double finalTargetSpeed = 30;
speedOutput = speedPid.compute(dt, currentVelocity, finalTargetSpeed);
speedOutput /= 3.6; // Convert to m/s related space
speedOutput /= properties.maxVehicleAccel; // Convert to [0:1] actuator range
sendMessage(sendTime, steeringMsg, turnOutput);
sendMessage(sendTime, accelMsg, speedOutput);
}
......@@ -246,20 +248,21 @@ public class JavaAutopilot extends EEComponent {
currSeg.initFromTraj(index);
boolean hasNext = index + 2 < trajX.length;
if (hasNext){
nextSeg.initFromTraj(index+1);
if (hasNext) {
nextSeg.initFromTraj(index + 1);
// Eval junction turn
double junctionAngle = Math.acos(IPM.dot(currSeg.dir, nextSeg.dir));
double junctionSign = Math.signum(IPM.dot(normal, nextSeg.dir));
double maxRadius = junctionAngle == 0 ? Double.POSITIVE_INFINITY : MAX_DEVIATION / ((1/Math.cos(junctionAngle*0.5)) - 1);
double maxDistToCorner = Math.sqrt(MAX_DEVIATION * (MAX_DEVIATION + maxRadius*2));
double maxRadius = junctionAngle == 0 ? Double.POSITIVE_INFINITY
: MAX_DEVIATION / ((1 / Math.cos(junctionAngle * 0.5)) - 1);
double maxDistToCorner = Math.sqrt(MAX_DEVIATION * (MAX_DEVIATION + maxRadius * 2));
// Check if "in turn"
if (currSeg.projDistToEnd <= maxDistToCorner){
if (target == 0){
if (currSeg.projDistToEnd <= maxDistToCorner) {
if (target == 0) {
target = 1;
//System.out.println("Following NEXT segment");
// System.out.println("Following NEXT segment");
}
return nextSeg;
}
......@@ -269,59 +272,59 @@ public class JavaAutopilot extends EEComponent {
return currSeg;
}
final Vec2 normal = new Vec2();
final Vec2 dir = new Vec2();
final Vec2 delta = new Vec2();
final Vec2 point = new Vec2();
final Vec2 lastPoint = new Vec2();
transient final Vec2 normal = new Vec2();
transient final Vec2 dir = new Vec2();
transient final Vec2 delta = new Vec2();
transient final Vec2 point = new Vec2();
transient final Vec2 lastPoint = new Vec2();
/**
* Returns the index of the closest point in "traj"
* or the index of the start point of the closest segment
* or -1 if there is no trajectory
* Returns the index of the closest point in "traj" or the index of the start
* point of the closest segment or -1 if there is no trajectory
*/
private int getNearestSegment(Vec2 pos) {
if (trajX == null || trajY == null) return -1;
if (trajX == null || trajY == null)
return -1;
double currentNearestDistance = Double.POSITIVE_INFINITY;
int closestIndex = -1;
int count = trajX.length;
double dist;
boolean hasLastPoint = false;
for (int i = 0; i < count; i++){
for (int i = 0; i < count; i++) {
getTrajPoint(i, point);
if (hasLastPoint){
if (hasLastPoint) {
// Check segment
// 1) Get segment "normal"
IPM.subtractTo(dir, point, lastPoint);
// Manual normalization to keep the length
double length = dir.magnitude();
if (length > 0.001){
IPM.multiply(dir, 1/length);
if (length > 0.001) {
IPM.multiply(dir, 1 / length);
} else {
dir.set(Double.NaN,Double.NaN);
dir.set(Double.NaN, Double.NaN);
}
// 2) check if in segment bounds
IPM.subtractTo(delta, pos, lastPoint);
double projPos = IPM.dot(dir, delta);
if (projPos > 0 && projPos < length) {
// 3) get distance
normal.set(-dir.y, dir.x);
dist = Math.abs(IPM.dot(normal, delta));
if (dist < currentNearestDistance){
if (dist < currentNearestDistance) {
currentNearestDistance = dist;
closestIndex = i-1;
closestIndex = i - 1;
}
}
}
//Check point
// Check point
dist = point.distance(pos);
if (dist < currentNearestDistance){
if (dist < currentNearestDistance) {
currentNearestDistance = dist;
closestIndex = i;
}
......@@ -332,7 +335,7 @@ public class JavaAutopilot extends EEComponent {
return closestIndex;
}
void getTrajPoint(int index, Vec2 target){
void getTrajPoint(int index, Vec2 target) {
target.set(trajX[index], trajY[index]);
}
......
......@@ -3,33 +3,48 @@ package de.rwth.montisim.simulation.eecomponents.autopilots;
import java.time.Duration;
import de.rwth.montisim.simulation.vehicle.componentbuilders.ComputerComponentBuilder;
import de.rwth.montisim.simulation.vehicle.componentbuilders.ComputerComponentProperties;
import de.rwth.montisim.commons.utils.json.Typed;
import de.rwth.montisim.simulation.eesimulator.components.EEComponentProperties;
import de.rwth.montisim.simulation.eesimulator.components.EEComponentType;
import de.rwth.montisim.simulation.vehicle.VehicleBuilder;
public class JavaAutopilotProperties extends ComputerComponentProperties {
public static final String COMPUTER_TYPE = "JavaAutopilot";
@Typed(JavaAutopilotProperties.TYPE)