diff --git a/.gitignore b/.gitignore index 2f7896d1d1365eafb0da03d9fe456fac81408487..0dd7edcf1e7b80014faee135efac41583b1ba7de 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,4 @@ -target/ +Bumpbot/target/ +Autopilot/target/ +Autopilot/models/.vscode +improved-array-handling-test/target/ diff --git a/Autopilot/Autopilot.json b/Autopilot/Autopilot.json new file mode 100644 index 0000000000000000000000000000000000000000..98b186c881def021a413046fc9a6d50633d3a0b2 --- /dev/null +++ b/Autopilot/Autopilot.json @@ -0,0 +1,7 @@ +{ + "modelsDir": "Autopilot/models/", + "outputDir": "Autopilot/target/", + "rootModel": "wrapper.wrapper", + "generators": ["cpp", "roscpp"] +} + diff --git a/Autopilot/README.md b/Autopilot/README.md new file mode 100644 index 0000000000000000000000000000000000000000..d388ebf1094d88063580addc1fb300f87268be05 --- /dev/null +++ b/Autopilot/README.md @@ -0,0 +1,21 @@ + +## How to use the wrapper: + +1. Build the EMAM Component. See [this page](https://git.rwth-aachen.de/monticore/EmbeddedMontiArc/applications/carlacomponents/blob/master/README.md) for more information. + +2. Install (and start) the carla-ros-bridge, as described [here](https://github.com/carla-simulator/ros-bridge). + +3. Install (and start) the carla-waypoint-publisher, as described [here](https://github.com/carla-simulator/ros-bridge/tree/master/carla_waypoint_publisher). + +4. Start the python script by running: + + python converter.py + + on your shell while in the containing directory. + +5. Start the executable generated in step one by running: + + cd ./target/install/bin + ./Coordinator_wrapper_wrapper + + while in the main directory of this project. \ No newline at end of file diff --git a/Autopilot/converter.py b/Autopilot/converter.py new file mode 100644 index 0000000000000000000000000000000000000000..e86fd8a900440850a4991c8c7f32d33c9c513bc4 --- /dev/null +++ b/Autopilot/converter.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python +import rospy +from nav_msgs.msg import Path +from std_msgs.msg import Float64MultiArray, Int32 + +rospy.init_node('converter', anonymous=True) +trajectory_x_pub = rospy.Publisher('autopilot/trajectory_x', Float64MultiArray) +trajectory_y_pub = rospy.Publisher('autopilot/trajectory_y', Float64MultiArray) +length_pub = rospy.Publisher('autopilot/trajectory_length', Int32) +rate = rospy.Rate(10) # 10hz + +def callback(path): + + trajectory_x = Float64MultiArray() + trajectory_y = Float64MultiArray() + trajectory_length = min(100, len(path.poses)) + + del trajectory_x.data[:] + del trajectory_y.data[:] + + for i in range(0, (trajectory_length)): + (trajectory_x.data).append(round((((path.poses)[i]).pose.position.x),2)) + (trajectory_y.data).append(round((((path.poses)[i]).pose.position.y),2)) + if (trajectory_length < 100): + for j in range(trajectory_length, 100): + (trajectory_x.data).append(0.00) + (trajectory_y.data).append(0.00) + + #rospy.loginfo(trajectory_length) + length_pub.publish(trajectory_length) + + #rospy.loginfo(trajectory_x) + trajectory_x_pub.publish(trajectory_x) + + #rospy.loginfo(trajectory_y) + trajectory_y_pub.publish(trajectory_y) + + + +def converter(): + + rospy.Subscriber("/carla/ego_vehicle/waypoints", Path, callback) + + #rospy.spin() + + + +if __name__ == '__main__': + try: + while not rospy.is_shutdown(): + converter() + rate.sleep() + except rospy.ROSInterruptException: + pass diff --git a/Autopilot/models/.vscode/launch.json b/Autopilot/models/.vscode/launch.json new file mode 100644 index 0000000000000000000000000000000000000000..2ce0e0801199e64d869f4368bbae99f140fb52f8 --- /dev/null +++ b/Autopilot/models/.vscode/launch.json @@ -0,0 +1,12 @@ +{ + "configurations": [ + { + "type": "emam", + "request": "launch", + "name": "Wrapper.emam", + "program": "wrapper.wrapper", + "stopOnEntry": true, + "modelBase": "${workspaceFolder}" + } + ] +} \ No newline at end of file diff --git a/Autopilot/models/Wrapper.tag b/Autopilot/models/Wrapper.tag new file mode 100644 index 0000000000000000000000000000000000000000..588273ed7ac123e1a084fe1cd2a57b04c1a48943 --- /dev/null +++ b/Autopilot/models/Wrapper.tag @@ -0,0 +1,21 @@ +package wrapper; +conforms to de.monticore.lang.monticar.generator.roscpp.RosToEmamTagSchema; + +tags Wrapper{ + + tag wrapper.trajectory_length with RosConnection = {topic = (/autopilot/trajectory_length, std_msgs/Int32)}; + tag wrapper.trajectory_x with RosConnection = {topic = (/autopilot/trajectory_x, std_msgs/Float64MultiArray)}; + tag wrapper.trajectory_y with RosConnection = {topic = (/autopilot/trajextory_y, std_msgs/Float64MultiArray)}; + + tag wrapper.vehicle_status with RosConnection = {topic = (/carla/ego_vehicle/vehicle_status, carla_msgs/CarlaEgoVehicleStatus)}; + tag wrapper.vehicle_control with RosConnection = {topic = (/carla/ego_vehicle/vehicle_control_cmd, carla_msgs/CarlaEgoVehicleControl)}; + + tag calculateWrappedValues.vehicle_status with RosConnection; + tag calculateWrappedValues.vehicle_control with RosConnection; + + tag autopilot.trajectory_length with RosConnection; + tag autopilot.trajectory_x with RosConnection; + tag autopilot.trajectory_y with RosConnection; + + tag wrapper.test with RosConnection = {topic = (/test, std_msgs/Float64), msgField=data}; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/Autopilot.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/Autopilot.emam new file mode 100644 index 0000000000000000000000000000000000000000..38bc832266652a98010a16193db2db20f4e03164 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/Autopilot.emam @@ -0,0 +1,79 @@ +package autopilot.de.rwth.armin.modeling.autopilot; + +import autopilot.de.rwth.armin.modeling.autopilot.common.*; +import autopilot.de.rwth.armin.modeling.autopilot.common.Compass2CurrentDirection; +import autopilot.de.rwth.armin.modeling.autopilot.behavior.*; +import autopilot.de.rwth.armin.modeling.autopilot.motion.*; + +component Autopilot { + 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; + + instance Compass2CurrentDirection c2cd; + instance BehaviorGeneration behaviorGeneration; + instance MotionPlanning motionPlanning; + + 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 currentVelocity -> motionPlanning.currentVelocity; + connect c2cd.currentDirectionX -> motionPlanning.currentDirectionX; + connect c2cd.currentDirectionY -> motionPlanning.currentDirectionY; + connect behaviorGeneration.desiredDirectionX -> motionPlanning.desiredDirectionX; + connect behaviorGeneration.desiredDirectionY -> motionPlanning.desiredDirectionY; + connect behaviorGeneration.signedDistanceToTrajectory -> motionPlanning.signedDistanceToTrajectory; + connect behaviorGeneration.desiredVelocity -> motionPlanning.desiredVelocity; + + connect motionPlanning.engine -> engine; + connect motionPlanning.steering -> steering; + connect motionPlanning.brakes -> brakes; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/Autopilot.emv b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/Autopilot.emv new file mode 100644 index 0000000000000000000000000000000000000000..e96cde43f92077b75b9b26ee79ddaf5259aed104 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/Autopilot.emv @@ -0,0 +1,131 @@ +package autopilot.de.rwth.armin.modeling.autopilot; + + +view Autopilot { + component Autopilot{ + ports in ? currentVelocity, + in ? compass, + in ? currentPosX, + in ? currentPosY, + in ? trajectory_length, + in ? trajectory_x, + in ? trajectory_y, + out ? Engine, + out ? Brakes, + out ? Steering; + + component Compass2CurrentDirection{ + ports in ? ?, + out ? ?, + out ? ?; + } + instance Compass2CurrentDirection comp; + + component BehaviorGeneration{ + ports in ? ?, + in ? ?, + in ? ?, + in ? ?, + in ? ?, + in ? ?, + in ? ?, + out ? ?; + } + instance BehaviorGeneration behavior; + + component CalculateMotionCommands{ + ports in ? ?, + in ? ?, + in ? ?, + in ? ?, + in ? ?, + out ? ?; + } + instance CalculateMotionCommands calcMotion; + + component TrimPath{ + ports in ? ?, + in ? ?, + in ? ?, + in ? ?, + in ? ?, + out ? ?; + } + instance TrimPath trim; + + component MotionPlanning{ + ports in ? ?, + in ? ?, + out ? ?; + } + instance MotionPlanning motion; + + component CalculateEngineAndBrakes{ + ports in ? ?, + in ? ?, + out ? ?; + } + instance CalculateEngineAndBrakes calcEngine; + + component KeepDirection{ + ports in ? ?, + out ? ?; + } + instance KeepDirection keep; + + component SteeringAngleCorrection{ + ports in ? ?, + out ? ?; + } + instance SteeringAngleCorrection steer; + + component EnsureBounds{ + ports in ? ?, + out ? ?; + } + instance EnsureBounds ensure; + + connect currentVelocity -> motion; + connect compass -> comp; + connect currentPosX -> behavior; + connect currentPosY -> behavior; + connect trajectory_length -> behavior; + connect trajectory_x -> behavior; + connect trajectory_y -> behavior; + + connect comp -> behavior; + connect comp -> behavior; + + connect behavior -> calcMotion; + connect behavior -> calcMotion; + connect behavior -> calcMotion; + connect behavior -> calcMotion; + connect behavior -> trim; + connect behavior -> trim; + connect behavior -> trim; + connect behavior -> trim; + connect behavior -> trim; + + connect trim -> calcMotion; + + connect calcMotion -> behavior; + + connect behavior -> motion; + + connect motion -> calcEngine; + connect motion -> calcEngine; + + connect calcEngine -> keep; + + connect keep -> steer; + + connect steer -> ensure; + + connect ensure -> motion; + + connect motion -> Engine; + connect motion -> Brakes; + connect motion -> Steering; + + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/Tests.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/Tests.emam new file mode 100644 index 0000000000000000000000000000000000000000..36a31348bc53e878f7526811393f52786142e0a2 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/Tests.emam @@ -0,0 +1,9 @@ +package autopilot.de.rwth.armin.modeling.autopilot; + +component Tests { + port in B in1, out B out1; + + instance autopilot.de.rwth.armin.modeling.autopilot.common.Tests t1; + instance autopilot.de.rwth.armin.modeling.autopilot.motion.Tests t2; + instance autopilot.de.rwth.armin.modeling.autopilot.behavior.Tests t3; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/basicComp/PID.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/basicComp/PID.emam new file mode 100644 index 0000000000000000000000000000000000000000..57a273ba8098506b05bb356d37ed26debdcddf9d --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/basicComp/PID.emam @@ -0,0 +1,29 @@ +package autopilot.de.rwth.armin.modeling.autopilot.basicComp; + +component PID{ +port + in Q desiredVel, + in Q currentVel, + in Q timeInc, + + out Q engineVal, + out Q breakesVal; + + implementation Math { + Q diff = desiredVel - currentVel; + static Q totalTime = 0.0; + totalTime = totalTime + timeInc; + + if totalTime > 100 + engineVal = 0.0; + breakesVal = 2.5; + elseif diff > 0 + engineVal = 2.5; + breakesVal = 0.0; + else + engineVal = 0.0; + breakesVal = 2.5; + end + } + +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/basicComp/PID.emv b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/basicComp/PID.emv new file mode 100644 index 0000000000000000000000000000000000000000..938a449b456eb70c9a0726f2789c2de923286d50 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/basicComp/PID.emv @@ -0,0 +1,11 @@ +package autopilot.de.rwth.armin.modeling.autopilot.basicComp; + + +view PID { + component PID{ + ports in ? currentSpeed, + in ? desiredSpeed, + out ? Engine, + out ? Brakes; + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/basicComp/PID1.stream b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/basicComp/PID1.stream new file mode 100644 index 0000000000000000000000000000000000000000..fed02722d838e5818fd5c407528b0bb1a1e5e2e4 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/basicComp/PID1.stream @@ -0,0 +1,10 @@ +package autopilot.de.rwth.armin.modeling.autopilot.basicComp; + +stream PID1 for PID { + desiredVel: 5 tick 5 tick 5 tick 5; + currentVel: 0 tick 4 tick 5 tick 10; + timeInc : 0.001 tick 0.0001 tick 0.0001 tick 0.0001; + + engineVal: 2.5 tick 2.5 tick - tick 0.0; + breakesVal: 0 tick 0 tick - tick 2.5; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/BehaviorGeneration.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/BehaviorGeneration.emam new file mode 100644 index 0000000000000000000000000000000000000000..a752dbe3f83195789f62ef40e357d6214a6ec124 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/BehaviorGeneration.emam @@ -0,0 +1,39 @@ +package autopilot.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; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/CalculateMotionCommands.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/CalculateMotionCommands.emam new file mode 100644 index 0000000000000000000000000000000000000000..813e8cb076a111e86c023f3731a6355b2569a1d0 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/CalculateMotionCommands.emam @@ -0,0 +1,69 @@ +package autopilot.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; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/Distance2Velocity.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/Distance2Velocity.emam new file mode 100644 index 0000000000000000000000000000000000000000..37cf25b9535ed976330b32817edbd470b90eee89 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/Distance2Velocity.emam @@ -0,0 +1,35 @@ +package autopilot.de.rwth.armin.modeling.autopilot.behavior; + +import autopilot.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; + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/DriveToFirstPosition.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/DriveToFirstPosition.emam new file mode 100644 index 0000000000000000000000000000000000000000..8d385788802524bd2df974e8aed79f332419a889 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/DriveToFirstPosition.emam @@ -0,0 +1,38 @@ +package autopilot.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 + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/FollowTrajectory.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/FollowTrajectory.emam new file mode 100644 index 0000000000000000000000000000000000000000..480ef2ab19b650c7c4769846bc80776cece53427 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/FollowTrajectory.emam @@ -0,0 +1,134 @@ +package autopilot.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 + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/IsDriveToFirstPosition.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/IsDriveToFirstPosition.emam new file mode 100644 index 0000000000000000000000000000000000000000..5de20a34a04d4c048a0ff6147b3ed78f40438c35 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/IsDriveToFirstPosition.emam @@ -0,0 +1,28 @@ +package autopilot.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 + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/Selector.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/Selector.emam new file mode 100644 index 0000000000000000000000000000000000000000..1a2a135a8c4272f44fbf39846538f747574e1b0f --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/Selector.emam @@ -0,0 +1,33 @@ +package autopilot.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 + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/Tests.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/Tests.emam new file mode 100644 index 0000000000000000000000000000000000000000..35ad3106d8b3673e04b960ae81d8d44e913b5b36 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/Tests.emam @@ -0,0 +1,7 @@ +package autopilot.de.rwth.armin.modeling.autopilot.behavior; + +component Tests { + port in B in1, out B out1; + + instance TrimPath trimPath; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/TrimPath.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/TrimPath.emam new file mode 100644 index 0000000000000000000000000000000000000000..dcb892d0ec072ac54c478a418e2bfb2963e3c9b2 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/TrimPath.emam @@ -0,0 +1,81 @@ +package autopilot.de.rwth.armin.modeling.autopilot.behavior; + +import autopilot.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, + 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 Z (0 : 100) trimmedTrajectoryLength, + out Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryX, + out Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryY; + + implementation Math { + trimmedTrajectoryLength = 0; + if plannedTrajectoryLength == 1 + trimmedTrajectoryLength = 1; + trimmedTrajectoryX(1,1) = plannedTrajectoryX(1,1); + trimmedTrajectoryY(1,1) = plannedTrajectoryY(1,1); + elseif plannedTrajectoryLength > 1 + Q closestSegmentIndex = -1; + Q closestSegmentDistance = -1.0; + Q lastSegmentIndex = plannedTrajectoryLength - 1; + for i = 1:lastSegmentIndex + Q p1x = plannedTrajectoryX(1,i); + Q p1y = plannedTrajectoryY(1,i); + Q p2x = plannedTrajectoryX(1,i+1); + Q p2y = plannedTrajectoryY(1,i+1); + // projection + Q vx = currentPositionX - p1x; + Q vy = currentPositionY - p1y; + Q v12x = p2x - p1x; + Q v12y = p2y - p1y; + Q k = ( vx*v12x + vy*v12y ) / ( v12x*v12x + v12y*v12y ); + Q projection_x = p1x + k * v12x; + Q projection_y = p1y + k * v12y; + Q is_projection_on_segment = ((p1x-projection_x) * (p2x-projection_x) <= 0) && ((p1y-projection_y) * (p2y-projection_y) <= 0); + if (is_projection_on_segment) + Q d_proj_sqr = (currentPositionX-projection_x)*(currentPositionX-projection_x) + (currentPositionY-projection_y)*(currentPositionY-projection_y); + Q d_proj = sqrt( d_proj_sqr ); + if (closestSegmentDistance < 0) || (d_proj < closestSegmentDistance) + closestSegmentIndex = i; + closestSegmentDistance = d_proj; + trimmedTrajectoryX(1,1) = projection_x; + trimmedTrajectoryY(1,1) = projection_y; + end + else + Q d1_sqr = (currentPositionX-p1x)*(currentPositionX-p1x) + (currentPositionY-p1y)*(currentPositionY-p1y); + Q d1 = sqrt( d1_sqr ); + Q d2_sqr = (currentPositionX-p2x)*(currentPositionX-p2x) + (currentPositionY-p2y)*(currentPositionY-p2y); + Q d2 = sqrt( d2_sqr ); + Q d_min = min(d1,d2); + if (closestSegmentDistance < 0) || (d_min < closestSegmentDistance) + closestSegmentIndex = i; + closestSegmentDistance = d_min; + trimmedTrajectoryX(1,1) = projection_x; + trimmedTrajectoryY(1,1) = projection_y; + end + end + end + if closestSegmentIndex > -1 + Q currentFree = 2; // 1st point is always the projection + Q start = closestSegmentIndex + 1; + for i = start:plannedTrajectoryLength + trimmedTrajectoryX(1,currentFree) = plannedTrajectoryX(1,i); + trimmedTrajectoryY(1,currentFree) = plannedTrajectoryY(1,i); + currentFree += 1; + end + trimmedTrajectoryLength = currentFree - 1; + end + end + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/BehaviorGeneration.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/BehaviorGeneration.emam new file mode 100644 index 0000000000000000000000000000000000000000..cb79f5f76d5950deb347a916de60db98f6128f1c --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/BehaviorGeneration.emam @@ -0,0 +1,39 @@ +package autopilot.de.rwth.armin.modeling.autopilot.behavior2; + +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; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/CalculateMotionCommands.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/CalculateMotionCommands.emam new file mode 100644 index 0000000000000000000000000000000000000000..57e5df653b082d427bcc4d68885433c55482195a --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/CalculateMotionCommands.emam @@ -0,0 +1,69 @@ +package autopilot.de.rwth.armin.modeling.autopilot.behavior2; + +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; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/Distance2Velocity.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/Distance2Velocity.emam new file mode 100644 index 0000000000000000000000000000000000000000..98ce549a100913c8826570fec1bf4b1f6d561cfb --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/Distance2Velocity.emam @@ -0,0 +1,33 @@ +package autopilot.de.rwth.armin.modeling.autopilot.behavior2; + +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 = 17.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; + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/DriveToFirstPosition.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/DriveToFirstPosition.emam new file mode 100644 index 0000000000000000000000000000000000000000..c059d9a9b80ba83bb42eed186e8b8ca379e04d49 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/DriveToFirstPosition.emam @@ -0,0 +1,38 @@ +package autopilot.de.rwth.armin.modeling.autopilot.behavior2; + +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 = 17.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 + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/FollowTrajectory.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/FollowTrajectory.emam new file mode 100644 index 0000000000000000000000000000000000000000..c66cf717a0ef241f3b54a22b676976632cb83697 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/FollowTrajectory.emam @@ -0,0 +1,134 @@ +package autopilot.de.rwth.armin.modeling.autopilot.behavior2; + +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 = 17.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 + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/IsDriveToFirstPosition.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/IsDriveToFirstPosition.emam new file mode 100644 index 0000000000000000000000000000000000000000..e0d2fe60b699aff5905ad5d1c87e6b12de19fa72 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/IsDriveToFirstPosition.emam @@ -0,0 +1,28 @@ +package autopilot.de.rwth.armin.modeling.autopilot.behavior2; + +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 + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/Selector.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/Selector.emam new file mode 100644 index 0000000000000000000000000000000000000000..fff1d63389b9f82639e5edafe0c31183cea0bd4e --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/Selector.emam @@ -0,0 +1,33 @@ +package autopilot.de.rwth.armin.modeling.autopilot.behavior2; + +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 + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/TrimPath.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/TrimPath.emam new file mode 100644 index 0000000000000000000000000000000000000000..48ede7bde8ada5faad0a836eaf5279885e7b4083 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior2/TrimPath.emam @@ -0,0 +1,74 @@ +package autopilot.de.rwth.armin.modeling.autopilot.behavior2; + +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, + 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 Z (0 : 100) trimmedTrajectoryLength, + out Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryX, + out Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryY; + + implementation Math { + trimmedTrajectoryLength = 0; + if plannedTrajectoryLength == 1 + trimmedTrajectoryLength = 1; + trimmedTrajectoryX(1,1) = plannedTrajectoryX(1,1); + trimmedTrajectoryY(1,1) = plannedTrajectoryY(1,1); + elseif plannedTrajectoryLength > 1 + Q closestSegmentIndex = -1; + Q closestSegmentDistance = -1.0; + Q lastSegmentIndex = plannedTrajectoryLength - 1; + for i = 1:lastSegmentIndex + Q p1x = plannedTrajectoryX(1,i); + Q p1y = plannedTrajectoryY(1,i); + Q p2x = plannedTrajectoryX(1,i+1); + Q p2y = plannedTrajectoryY(1,i+1); + // projection + Q vx = currentPositionX - p1x; + Q vy = currentPositionY - p1y; + Q v12x = p2x - p1x; + Q v12y = p2y - p1y; + Q k = ( vx*v12x + vy*v12y ) / ( v12x*v12x + v12y*v12y ); + Q projection_x = p1x + k * v12x; + Q projection_y = p1y + k * v12y; + Q is_projection_on_segment = ((p1x-projection_x) * (p2x-projection_x) <= 0) && ((p1y-projection_y) * (p2y-projection_y) <= 0); + if (is_projection_on_segment) + Q d_proj_sqr = (currentPositionX-projection_x)*(currentPositionX-projection_x) + (currentPositionY-projection_y)*(currentPositionY-projection_y); + Q d_proj = sqrt( d_proj_sqr ); + if (closestSegmentDistance < 0) || (d_proj < closestSegmentDistance) + closestSegmentIndex = i; + closestSegmentDistance = d_proj; + trimmedTrajectoryX(1,1) = projection_x; + trimmedTrajectoryY(1,1) = projection_y; + end + else + Q d1_sqr = (currentPositionX-p1x)*(currentPositionX-p1x) + (currentPositionY-p1y)*(currentPositionY-p1y); + Q d1 = sqrt( d1_sqr ); + Q d2_sqr = (currentPositionX-p2x)*(currentPositionX-p2x) + (currentPositionY-p2y)*(currentPositionY-p2y); + Q d2 = sqrt( d2_sqr ); + Q d_min = min(d1,d2); + if (closestSegmentDistance < 0) || (d_min < closestSegmentDistance) + closestSegmentIndex = i; + closestSegmentDistance = d_min; + trimmedTrajectoryX(1,1) = projection_x; + trimmedTrajectoryY(1,1) = projection_y; + end + end + end + if closestSegmentIndex > -1 + Q currentFree = 2; // 1st point is always the projection + Q start = closestSegmentIndex + 1; + for i = start:plannedTrajectoryLength + trimmedTrajectoryX(1,currentFree) = plannedTrajectoryX(1,i); + trimmedTrajectoryY(1,currentFree) = plannedTrajectoryY(1,i); + currentFree += 1; + end + trimmedTrajectoryLength = currentFree - 1; + end + end + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Abs.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Abs.emam new file mode 100644 index 0000000000000000000000000000000000000000..1cc3624654459612834da330832e0b0d3e0012d0 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Abs.emam @@ -0,0 +1,15 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +component Abs { + port + in Q input, + out Q output; + + implementation Math { + if input < 0 + output = (-1) * input; + else + output = input; + end + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Abs.stream b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Abs.stream new file mode 100644 index 0000000000000000000000000000000000000000..1fdc918000c69ce86dd94ebc2e08d8898b62a0f1 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Abs.stream @@ -0,0 +1,6 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +stream Abs for Abs { + input: -100 tick -0.1 tick 0.0 tick 0.01 tick 20; + output: 100 +/- 0.001 tick 0.1 +/- 0.001 tick 0.0 +/- 0.001 tick 0.01 +/- 0.001 tick 20 +/- 0.001; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Compass2CurrentDirection.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Compass2CurrentDirection.emam new file mode 100644 index 0000000000000000000000000000000000000000..8b5740d843c485258c03fefa162e33e6b6b60500 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Compass2CurrentDirection.emam @@ -0,0 +1,14 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +component Compass2CurrentDirection { + port + in Q (-oo : 0.001 : oo) compass, + out Q (-1.0 m : 0.001 m : 1.0 m) currentDirectionX, + out Q (-1.0 m : 0.001 m : 1.0 m) currentDirectionY; + + implementation Math { + Q angle = compass + 0.5 * M_PI; + currentDirectionX = cos(angle); + currentDirectionY = sin(angle); + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Compass2CurrentDirection.stream b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Compass2CurrentDirection.stream new file mode 100644 index 0000000000000000000000000000000000000000..45da7e4fef170d4eefec9f4f7a8b455e35172961 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Compass2CurrentDirection.stream @@ -0,0 +1,7 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +stream Compass2CurrentDirection for Compass2CurrentDirection { + compass: 0.0 tick 0.8 tick 1.57 tick 1.9 tick 3.141 tick 3.3 tick -0.3 ; + current_direction_x: 0.0 +/- 0.001 tick -0.717 +/- 0.001 tick -0.999 +/- 0.001 tick -0.946 +/- 0.001 tick 0.0 +/- 0.001 tick 0.157 +/- 0.001 tick 0.295 +/- 0.001 ; + current_direction_y: 1.0 +/- 0.001 tick 0.696 +/- 0.001 tick 0.0 +/- 0.001 tick -0.323 +/- 0.001 tick -0.999 +/- 0.001 tick -0.987 +/- 0.001 tick 0.955 +/- 0.001 ; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/EnsureBounds.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/EnsureBounds.emam new file mode 100644 index 0000000000000000000000000000000000000000..c39260e8804ff5bb2d5062c58321ced7d5dd967b --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/EnsureBounds.emam @@ -0,0 +1,19 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +component EnsureBounds { + port + in Q lowerBound, + in Q upperBound, + in Q input, + out Q output; + + implementation Math { + if input < lowerBound + output = lowerBound; + elseif input > upperBound + output = upperBound; + else + output = input; + end + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/EnsureBounds.stream b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/EnsureBounds.stream new file mode 100644 index 0000000000000000000000000000000000000000..7b3e56cdfc85ce4597a2ed67cef8b6c0b5206758 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/EnsureBounds.stream @@ -0,0 +1,8 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +stream EnsureBounds for EnsureBounds { + lowerBound: 1.0 tick 0.0 tick -3.0 tick 5.0; + upperBound: 2.0 tick 10.0 tick 3.0 tick 5.5; + input: 0.0 tick 15.0 tick 1.0 tick 5.3; + output: 1.0 tick 10.0 tick 1.0 tick 5.3; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/EnsureBoundsForBrakes.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/EnsureBoundsForBrakes.emam new file mode 100644 index 0000000000000000000000000000000000000000..2d9fb53dc7aeea4913c82bfe83929fd92a07f8b2 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/EnsureBoundsForBrakes.emam @@ -0,0 +1,12 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +component EnsureBoundsForBrakes { + port in Q input, out Q output; + + instance EnsureBounds eb; + + connect 0.0 -> eb.lowerBound; + connect 3.0 -> eb.upperBound; + connect input -> eb.input; + connect eb.output -> output; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/EnsureBoundsForEngine.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/EnsureBoundsForEngine.emam new file mode 100644 index 0000000000000000000000000000000000000000..aa811ac073ed814fa12daaba05e38da1e615f34a --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/EnsureBoundsForEngine.emam @@ -0,0 +1,12 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +component EnsureBoundsForEngine { + port in Q input, out Q output; + + instance EnsureBounds eb; + + connect 0.0 -> eb.lowerBound; + connect 2.5 -> eb.upperBound; + connect input -> eb.input; + connect eb.output -> output; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/EnsureBoundsForSteeringAngle.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/EnsureBoundsForSteeringAngle.emam new file mode 100644 index 0000000000000000000000000000000000000000..96031b040febc75e3805297f30f50be6a1db02d2 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/EnsureBoundsForSteeringAngle.emam @@ -0,0 +1,12 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +component EnsureBoundsForSteeringAngle { + port in Q input, out Q output; + + instance EnsureBounds eb; + + connect -0.785 -> eb.lowerBound; + connect 0.785 -> eb.upperBound; + connect input -> eb.input; + connect eb.output -> output; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Exponent.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Exponent.emam new file mode 100644 index 0000000000000000000000000000000000000000..03f57fb136a904a4802f5477fa74787b3217f8fe --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Exponent.emam @@ -0,0 +1,24 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +component Exponent { + port + // TODO convert to parameters later + in Q x1, + in Q y1, + in Q x2, + in Q y2, + + in Q arg, + out Q val; + + implementation Math { + if (arg <= x1) + val = y1; + elseif (arg >= x2) + val = y2; + else + Q k = log( (y1 / y2) ) / (x2 - x1); + val = y1 * exp(-k * (arg - x1)); + end + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Exponent.stream b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Exponent.stream new file mode 100644 index 0000000000000000000000000000000000000000..bba5d34b41086fd71882f8762e3e0e779b0340d7 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Exponent.stream @@ -0,0 +1,10 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +stream Exponent for Exponent { + x1: 1.0 tick 1.0 tick 1.0 tick 1.0 tick 1.0 tick 1.0 ; + y1: 1.0 tick 1.0 tick 1.0 tick 1.0 tick 1.0 tick 1.0 ; + x2: 5.0 tick 5.0 tick 5.0 tick 5.0 tick 5.0 tick 5.0 ; + y2: 50.0 tick 50.0 tick 50.0 tick 50.0 tick 50.0 tick 50.0 ; + arg: 0.0 tick 1.0 tick 1.5 tick 3.0 tick 5.0 tick 5.5 ; + val: 1.0 +/- 0.01 tick 1.0 +/- 0.01 tick 1.63 +/- 0.01 tick 7.07 +/- 0.01 tick 50.0 +/- 0.1 tick 50.0 +/- 0.1 ; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/PID.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/PID.emam new file mode 100644 index 0000000000000000000000000000000000000000..26128f787e8ddbb4989f0fe8d86e014085d8b767 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/PID.emam @@ -0,0 +1,25 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +component PID { + port + in Q paramP, + in Q paramI, + in Q paramD, + in Q paramDecayCoefficient, + in Q error, + out Q control; + + implementation Math { + static B isPrevErrorSpecified = 0; + static Q prevError = 0.0; + static Q acc = 0.0; + acc = paramDecayCoefficient * acc + error; + Q drv = 0; + if isPrevErrorSpecified + drv = error - prevError; + end + control = paramP * error + paramI * acc + paramD * drv; + prevError = error; + isPrevErrorSpecified = 1; + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/PID.stream b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/PID.stream new file mode 100644 index 0000000000000000000000000000000000000000..c38e52bff32718635572a2f2495c844446b81b94 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/PID.stream @@ -0,0 +1,10 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +stream PID for PID { + paramP: 1.0 tick 2.0 tick 2.0; + paramI: 0.0 tick 0.0 tick 3.0; + paramD: 0.0 tick 0.0 tick 5.0; + paramDecayCoefficient: 0.0 tick 0.0 tick 0.01; + error: 9.0 tick -3.0 tick 0.5; + control: 9.0 +/- 0.01 tick -6.0 +/- 0.01 tick 19.91 +/- 0.01; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/SignedAngleBetween.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/SignedAngleBetween.emam new file mode 100644 index 0000000000000000000000000000000000000000..f31b66c0eece1fab7709d5763685a1b499a57ff6 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/SignedAngleBetween.emam @@ -0,0 +1,34 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +component SignedAngleBetween { + port + in Q v1x, + in Q v1y, + in Q v2x, + in Q v2y, + + out Q angle; + + implementation Math { + static Q EPSILON = 0.001; + angle = 0.0; + Q norm1 = sqrt( v1x*v1x + v1y*v1y ); + Q norm2 = sqrt( v2x*v2x + v2y*v2y ); + if ( (norm1 > EPSILON) && (norm2 > EPSILON) ) + Q angle1 = -atan2(v1y, v1x); + Q c = cos(angle1); + Q s = sin(angle1); + // v2 rotated + Q v2xr = v2x * c - v2y * s; + Q v2yr = v2x * s + v2y * c; + Q angle2 = -atan2(v2yr, v2xr); + Q abs1 = abs(angle2); + Q abs2 = abs(abs1 - M_PI); + if (abs2 <= EPSILON) + angle = -M_PI; + else + angle = angle2; + end + end + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/SignedAngleBetween.stream b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/SignedAngleBetween.stream new file mode 100644 index 0000000000000000000000000000000000000000..f8db9c17a33c938bafa00819a4cbd361c1f80775 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/SignedAngleBetween.stream @@ -0,0 +1,9 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +stream SignedAngleBetween for SignedAngleBetween { + v1x: 1.0 tick 0.0 tick 1.0 tick 1.0 tick 1.0 tick 1.0 tick 0.0 tick 0.0 ; + v1y: 0.0 tick 1.0 tick 1.0 tick 1.0 tick 1.0 tick 1.0 tick 0.0 tick 0.0 ; + v2x: 0.0 tick -1.0 tick -1.0 tick 10.0 tick -10.0 tick 0.0 tick 1.0 tick 0.0 ; + v2y: -1.0 tick 0.0 tick 0.0 tick 2.0 tick -10.0 tick 0.0 tick 1.0 tick 0.0 ; + angle: 1.57 +/- 0.01 tick -1.57 +/- 0.01 tick -2.35 +/- 0.01 tick 0.58 +/- 0.01 tick -3.14 +/- 0.01 tick 0.0 +/- 0.01 tick 0.0 +/- 0.01 tick 0.0 +/- 0.01 ; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Sum.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Sum.emam new file mode 100644 index 0000000000000000000000000000000000000000..59a3d726b7292ec4b6e8fa32cd1b85d72a2aacd6 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Sum.emam @@ -0,0 +1,12 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +component Sum { + port + in Q t1, + in Q t2, + out Q result; + + implementation Math { + result = t1 + t2; + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Sum.stream b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Sum.stream new file mode 100644 index 0000000000000000000000000000000000000000..ae76a6fd4f756f8d033b776a0bfed7a3cfc7fc84 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Sum.stream @@ -0,0 +1,7 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +stream Sum for Sum { + t1: 1 tick 2 tick 3; + t2: -1 tick 0 tick 10; + result: 0.0 +/- 0.01 tick 2.0 +/- 0.01 tick 13.0 +/- 0.01; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Tests.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Tests.emam new file mode 100644 index 0000000000000000000000000000000000000000..3353aa2e23e3dca3a5391b888c5f01f45dc287d9 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/common/Tests.emam @@ -0,0 +1,14 @@ +package autopilot.de.rwth.armin.modeling.autopilot.common; + +component Tests { + port in B in1, out B out1; + + instance Abs abs; + instance EnsureBounds eb1; + instance EnsureBoundsForBrakes eb2; + instance EnsureBoundsForEngine eb3; + instance EnsureBoundsForSteeringAngle eb4; + instance PID pid; + instance SignedAngleBetween sab; + instance Sum sum; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculateEngineAndBrakes.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculateEngineAndBrakes.emam new file mode 100644 index 0000000000000000000000000000000000000000..03353d1bc3c2437e9e1943296b7a08028cc05285 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculateEngineAndBrakes.emam @@ -0,0 +1,37 @@ +package autopilot.de.rwth.armin.modeling.autopilot.motion; + +import autopilot.de.rwth.armin.modeling.autopilot.common.*; + +component CalculateEngineAndBrakes { + port + in Q currentVelocity, + in Q desiredVelocity, + out Q engine, + out Q brakes; + + instance CalculatePidParameters pidParams; + instance CalculatePidError pidError; + instance Abs abs1; + instance PID pid; + instance DecideEngineOrBrakes engineOrBrakes; + + connect currentVelocity -> pidParams.currentVelocity; + connect desiredVelocity -> pidParams.desiredVelocity; + + connect currentVelocity -> pidError.currentVelocity; + connect desiredVelocity -> pidError.desiredVelocity; + + connect pidParams.paramP -> pid.paramP; + connect pidParams.paramI -> pid.paramI; + connect pidParams.paramD -> pid.paramD; + connect pidParams.paramDecayCoefficient -> pid.paramDecayCoefficient; + + connect pidError.error -> abs1.input; + connect abs1.output -> pid.error; + + connect pidError.error -> engineOrBrakes.error; + connect pid.control -> engineOrBrakes.controlSignal; + + connect engineOrBrakes.engine -> engine; + connect engineOrBrakes.brakes -> brakes; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculateEngineAndBrakes.emv b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculateEngineAndBrakes.emv new file mode 100644 index 0000000000000000000000000000000000000000..1ffba72f74b3e01896ff82ceb07c583073d0750a --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculateEngineAndBrakes.emv @@ -0,0 +1,72 @@ +package autopilot.de.rwth.armin.modeling.autopilot.motion; + + +view CalculateEngineAndBrakes{ + component CalculateEngineAndBrakes{ + ports in ? currentSpeed, + in ? desiredSpeed, + out ? Engine, + out ? Brakes; + + component CalculatePidParameters{ + ports in ? ?, + in ? ?, + out ? P, + out ? I, + out ? D, + out ? ParamC; + } + instance CalculatePidParameters calcPIDparam; + + component CalculatePidError{ + ports in ? ?, + in ? ?, + out ? Error; + } + instance CalculatePidError calcPIDerror; + + component Abs{ + ports in ? ?, + out ? ?; + } + instance Abs abs; + + component PID{ + ports in ? ?, + in ? ?, + in ? ?, + in ? ?, + in ? ?, + out ? Control; + } + instance PID pid; + + component DecideEngineOrBrakes{ + ports in ? ?, + in ? ?, + out ? ?, + out ? ?; + } + instance DecideEngineOrBrakes engineor; + + connect currentSpeed -> calcPIDerror; + connect currentSpeed -> calcPIDparam; + connect desiredSpeed -> calcPIDerror; + connect desiredSpeed -> calcPIDparam; + + connect calcPIDerror.Error -> abs; + connect calcPIDerror.Error -> engineor; + + connect calcPIDparam.P -> pid; + connect calcPIDparam.I -> pid; + connect calcPIDparam.D -> pid; + connect calcPIDparam.ParamC -> pid; + + connect abs -> pid; + + connect pid.Control -> engineor; + + connect engineor -> Engine; + connect engineor -> Brakes; + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculatePidError.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculatePidError.emam new file mode 100644 index 0000000000000000000000000000000000000000..668fb851b299517e8784661834d729d57992faa7 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculatePidError.emam @@ -0,0 +1,24 @@ +package autopilot.de.rwth.armin.modeling.autopilot.motion; + +component CalculatePidError { + port + in Q (0.0 m/s : 0.01 m/s : oo) currentVelocity, + in Q (0.0 m/s : 0.01 m/s : 13.0 m/s) desiredVelocity, + out Q error; + + implementation Math { + // TODO convert into parameter when parameters start working (bug #28) + static Q V_THRESHOLD_FOR_ERROR_LEAP = 1.5; + if (desiredVelocity <= 0.01) + error = -100.0; + elseif ((currentVelocity - desiredVelocity) > V_THRESHOLD_FOR_ERROR_LEAP) + // dv is treated as zero in order to increase error and force vehicle to decelerate better + error = -currentVelocity; + elseif ((desiredVelocity - currentVelocity) > V_THRESHOLD_FOR_ERROR_LEAP) + // increase error to force vehicle to accelerate more rapidly + error = desiredVelocity; + else + error = desiredVelocity - currentVelocity; + end + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculatePidError.stream b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculatePidError.stream new file mode 100644 index 0000000000000000000000000000000000000000..8de76dd7e43c7a07b83a671bb2c401842a49b7b0 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculatePidError.stream @@ -0,0 +1,7 @@ +package autopilot.de.rwth.armin.modeling.autopilot.motion; + +stream CalculatePidError for CalculatePidError { + currentVelocity: 1.0 tick 1.0 tick 1.0 tick 10.0 tick 8.0 tick 10.0 ; + desiredVelocity: 1.0 tick 0.0 tick 5.0 tick 3.0 tick 9.1 tick 9.5 ; + error: 0.0 +/- 0.01 tick -100.0 +/- 0.01 tick 5.0 +/- 0.01 tick -10.0 +/- 0.01 tick 1.1 +/- 0.01 tick -0.5 +/- 0.01 ; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculatePidParameters.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculatePidParameters.emam new file mode 100644 index 0000000000000000000000000000000000000000..8fd8a0ce12bb7424e5ae26e3a516701062e10e70 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculatePidParameters.emam @@ -0,0 +1,38 @@ +package autopilot.de.rwth.armin.modeling.autopilot.motion; + +component CalculatePidParameters { + port + in Q currentVelocity, + in Q desiredVelocity, + + out Q paramP, + out Q paramI, + out Q paramD, + out Q paramDecayCoefficient; + + implementation Math { + static Q MIN_VELOCITY = 0.5; + static Q MAX_VELOCITY = 13.0 - MIN_VELOCITY / 10.0; + static Q P_FOR_MIN_VELOCITY = 1.76703; + static Q P_FOR_MAX_VELOCITY = 3.29578; + + paramP = 0.0; + paramI = 0.0; + paramD = 0.0; + paramDecayCoefficient = 0.0; + + Q v = currentVelocity; + if (desiredVelocity < currentVelocity) + v = desiredVelocity; + end + + if (v < MIN_VELOCITY) + paramP = P_FOR_MIN_VELOCITY; + elseif (v > MAX_VELOCITY) + paramP = P_FOR_MAX_VELOCITY; + else + Q diff = v - MIN_VELOCITY; + paramP = P_FOR_MIN_VELOCITY + (P_FOR_MAX_VELOCITY - P_FOR_MIN_VELOCITY) * diff / (MAX_VELOCITY - MIN_VELOCITY); + end + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculatePidParameters.stream b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculatePidParameters.stream new file mode 100644 index 0000000000000000000000000000000000000000..6f775ba381fb6ed822f725daae82a06636d8bc69 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/CalculatePidParameters.stream @@ -0,0 +1,10 @@ +package autopilot.de.rwth.armin.modeling.autopilot.motion; + +stream CalculatePidParameters for CalculatePidParameters { + currentVelocity: 0.0 tick 1.0 tick 3.0 tick 13.0 tick 15.0 ; + desiredVelocity: 0.0 tick 2.0 tick 7.0 tick 13.0 tick 13.0 ; + paramP: 1.76 +/- 0.01 tick 1.82 +/- 0.01 tick 2.07 +/- 0.01 tick 3.29 +/- 0.01 tick 3.29 +/- 0.01 ; + paramI: 0.0 tick 0.0 tick 0.0 tick 0.0 tick 0.0 ; + paramD: 0.0 tick 0.0 tick 0.0 tick 0.0 tick 0.0 ; + paramDecayCoefficient: 0.0 tick 0.0 tick 0.0 tick 0.0 tick 0.0 ; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/DecideEngineOrBrakes.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/DecideEngineOrBrakes.emam new file mode 100644 index 0000000000000000000000000000000000000000..5278b49d2bdb2075d7b9900e8df7d19aa79718ee --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/DecideEngineOrBrakes.emam @@ -0,0 +1,19 @@ +package autopilot.de.rwth.armin.modeling.autopilot.motion; + +component DecideEngineOrBrakes { + port + in Q error, + in Q controlSignal, + out Q engine, + out Q brakes; + + implementation Math { + engine = 0.0; + brakes = 0.0; + if (error > 0) + engine = controlSignal; + else + brakes = controlSignal; + end + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/DecideEngineOrBrakes.stream b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/DecideEngineOrBrakes.stream new file mode 100644 index 0000000000000000000000000000000000000000..73b3d74f579f617cebba7d60da6b0601f46e2911 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/DecideEngineOrBrakes.stream @@ -0,0 +1,8 @@ +package autopilot.de.rwth.armin.modeling.autopilot.motion; + +stream DecideEngineOrBrakes for DecideEngineOrBrakes { + error: 1.0 tick -10.0 ; + controlSignal: 5.0 tick 20.0 ; + engine: 5.0 +/- 0.01 tick 0.0 +/- 0.01 ; + brakes: 0.0 +/- 0.01 tick 20.0 +/- 0.01 ; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/KeepDirection.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/KeepDirection.emam new file mode 100644 index 0000000000000000000000000000000000000000..38f92f63eea8d6c8d2a9c831bafdedb78bfde7ea --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/KeepDirection.emam @@ -0,0 +1,21 @@ +package autopilot.de.rwth.armin.modeling.autopilot.motion; + +import autopilot.de.rwth.armin.modeling.autopilot.common.SignedAngleBetween; + +component KeepDirection { + port + in Q currentDirectionX, + in Q currentDirectionY, + in Q desiredDirectionX, + in Q desiredDirectionY, + + out Q steeringAngle; + + instance SignedAngleBetween sab; + + connect currentDirectionX -> sab.v1x; + connect currentDirectionY -> sab.v1y; + connect desiredDirectionX -> sab.v2x; + connect desiredDirectionY -> sab.v2y; + connect sab.angle -> steeringAngle; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/MotionPlanning.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/MotionPlanning.emam new file mode 100644 index 0000000000000000000000000000000000000000..c73d66db2872abf0e6de5986fa38ec111c04ada8 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/MotionPlanning.emam @@ -0,0 +1,49 @@ +package autopilot.de.rwth.armin.modeling.autopilot.motion; + +import autopilot.de.rwth.armin.modeling.autopilot.common.*; + +component MotionPlanning { + port + in Q currentDirectionX, + in Q currentDirectionY, + in Q desiredDirectionX, + in Q desiredDirectionY, + in Q signedDistanceToTrajectory, + in Q (0.0 m/s : 0.01 m/s : oo m/s) currentVelocity, + in Q (0.0 m/s : 0.01 m/s : 13.0 m/s) desiredVelocity, + + out Q engine, + out Q steering, + out Q brakes; + + instance KeepDirection keepDirection; + instance SteeringAngleCorrection sac; + instance Sum sum1; + instance CalculateEngineAndBrakes engineAndBrakes; + instance EnsureBoundsForSteeringAngle boundsSteering; + instance EnsureBoundsForBrakes boundsBrakes; + instance EnsureBoundsForEngine boundsEngine; + + connect currentDirectionX -> keepDirection.currentDirectionX; + connect currentDirectionY -> keepDirection.currentDirectionY; + connect desiredDirectionX -> keepDirection.desiredDirectionX; + connect desiredDirectionY -> keepDirection.desiredDirectionY; + + connect keepDirection.steeringAngle -> sum1.t1; + connect signedDistanceToTrajectory -> sac.signedDistanceToTrajectory; + connect sac.steeringAngleCorrection -> sum1.t2; + + // brakes and engine + connect currentVelocity -> engineAndBrakes.currentVelocity; + connect desiredVelocity -> engineAndBrakes.desiredVelocity; + + // compose output + connect engineAndBrakes.engine -> boundsEngine.input; + connect boundsEngine.output -> engine; + + connect sum1.result -> boundsSteering.input; + connect boundsSteering.output -> steering; + + connect engineAndBrakes.brakes -> boundsBrakes.input; + connect boundsBrakes.output -> brakes; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/SteeringAngleCorrection.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/SteeringAngleCorrection.emam new file mode 100644 index 0000000000000000000000000000000000000000..57625b446fb5146dcc802eca6deda0a83eff80c6 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/SteeringAngleCorrection.emam @@ -0,0 +1,41 @@ +package autopilot.de.rwth.armin.modeling.autopilot.motion; + +component SteeringAngleCorrection { + port + in Q signedDistanceToTrajectory, + out Q steeringAngleCorrection; + + implementation Math { + // TODO convert into parameter when parameters start working (bug #28) + static Q MAX_STEERING_ANGLE = 0.785; + static Q EPSILON = 0.01; + // exponent params + static Q X1 = 0.1; + static Q Y1 = 0.01 * MAX_STEERING_ANGLE; + static Q X2 = 5.0; + static Q Y2 = 0.05 * MAX_STEERING_ANGLE; + static Q COEF_K = log( (Y1 / Y2) ) / (X2 - X1); + + steeringAngleCorrection = 0.0; + + Q dist = signedDistanceToTrajectory; + + if dist < 0 + dist = (-1) * dist; // dist = abs(signedDistanceToTrajectory) + end + + if (dist > EPSILON) + if (dist < X1) + steeringAngleCorrection = Y1; + elseif (dist > X2) + steeringAngleCorrection = Y2; + else + steeringAngleCorrection = Y1 * exp(-COEF_K * (dist - X1)); + end + // take sign into account + if (signedDistanceToTrajectory < 0) + steeringAngleCorrection *= -1.0; + end + end + } +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/SteeringAngleCorrection.stream b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/SteeringAngleCorrection.stream new file mode 100644 index 0000000000000000000000000000000000000000..6b1458de50060627ae59bfd8558286df3a89e9e9 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/SteeringAngleCorrection.stream @@ -0,0 +1,6 @@ +package autopilot.de.rwth.armin.modeling.autopilot.motion; + +stream SteeringAngleCorrection for SteeringAngleCorrection { + signedDistanceToTrajectory: 0.0 tick 0.05 tick 2.5 tick 5.0 tick 15.5 ; + steeringAngleCorrection: 0.0 +/- 0.001 tick 0.007 +/- 0.001 tick 0.017 +/- 0.001 tick 0.039 +/- 0.001 tick 0.039 +/- 0.001 ; +} diff --git a/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/Tests.emam b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/Tests.emam new file mode 100644 index 0000000000000000000000000000000000000000..5275d1a8c3cac9aa67873cfc5c767978fe4cf183 --- /dev/null +++ b/Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/motion/Tests.emam @@ -0,0 +1,13 @@ +package autopilot.de.rwth.armin.modeling.autopilot.motion; + +component Tests { + port in B in1, out B out1; + + instance CalculateEngineAndBrakes calculateEngineAndBrakes; + instance CalculatePidError calculatePidError; + instance CalculatePidParameters calculatePidParameters; + instance DecideEngineOrBrakes decideEngineOrBrakes; + instance KeepDirection keepDirection; + instance MotionPlanning motionPlanning; + instance SteeringAngleCorrection steeringAngleCorrection; +} diff --git a/Autopilot/models/autopilot/main.txt b/Autopilot/models/autopilot/main.txt new file mode 100644 index 0000000000000000000000000000000000000000..1444b135cdd1f45de51b103a5193d098b60ce2c1 --- /dev/null +++ b/Autopilot/models/autopilot/main.txt @@ -0,0 +1,24 @@ +// The main.txt will act as manifest for our EMA models +// It will specify important information and entry points into the model + + +// Specify the full qualified name to the main component and a name for the instantiation +// The instantiation name can deviate from the component +Main-Component-Instantiation: de.rwth.armin.modeling.autopilot.Autopilot myAutoPilotInst; + + +// Specify one or more model-paths, which contain models. +// They are split by ',' like this: [../../../../../src/test/resources, ../../../../../src/main/resources] +// The are always relative to main.txt +Model-Paths: [./] + + + +// This feature is implemented in EMA version > 0.1.0-SNAPSHOT +// Additionally when working on with the EMA language, load all your models with the methods: +// +// @param mainTxt Path to main.txt +// loadComponentFromMainTxt(String mainTxt) +// createSymTabFromMainTxt(String mainTxt) +// +// Both located in EmbeddedMontiArcModelLoader diff --git a/Autopilot/models/autopilot/resources/sample1results/execution0de.rwth.armin.modeling.autopilot.motion.calculatePidError.res b/Autopilot/models/autopilot/resources/sample1results/execution0de.rwth.armin.modeling.autopilot.motion.calculatePidError.res new file mode 100644 index 0000000000000000000000000000000000000000..f804ef144098b0b6a559e6be8e4bcf709f7551a5 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample1results/execution0de.rwth.armin.modeling.autopilot.motion.calculatePidError.res @@ -0,0 +1,3 @@ +currentVelocity : 4.50718e+006 +desiredVelocity : 7.46896 +error : -4.50718e+006 diff --git a/Autopilot/models/autopilot/resources/sample1results/execution1de.rwth.armin.modeling.autopilot.motion.calculatePidError.res b/Autopilot/models/autopilot/resources/sample1results/execution1de.rwth.armin.modeling.autopilot.motion.calculatePidError.res new file mode 100644 index 0000000000000000000000000000000000000000..dfac0fbb87caf3e1c707400423a4d217badecfef --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample1results/execution1de.rwth.armin.modeling.autopilot.motion.calculatePidError.res @@ -0,0 +1,3 @@ +currentVelocity : 8.5276e+006 +desiredVelocity : 7.3383 +error : -8.5276e+006 diff --git a/Autopilot/models/autopilot/resources/sample1results/execution2de.rwth.armin.modeling.autopilot.motion.calculatePidError.res b/Autopilot/models/autopilot/resources/sample1results/execution2de.rwth.armin.modeling.autopilot.motion.calculatePidError.res new file mode 100644 index 0000000000000000000000000000000000000000..839e35e2d7fe01d4bdc5fe5a205dffb26828823a --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample1results/execution2de.rwth.armin.modeling.autopilot.motion.calculatePidError.res @@ -0,0 +1,3 @@ +currentVelocity : 9.55952e+006 +desiredVelocity : 11.5264 +error : -9.55952e+006 diff --git a/Autopilot/models/autopilot/resources/sample1results/execution3de.rwth.armin.modeling.autopilot.motion.calculatePidError.res b/Autopilot/models/autopilot/resources/sample1results/execution3de.rwth.armin.modeling.autopilot.motion.calculatePidError.res new file mode 100644 index 0000000000000000000000000000000000000000..c98717a4f5e2e5f64b2694b44efb056899f50c4d --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample1results/execution3de.rwth.armin.modeling.autopilot.motion.calculatePidError.res @@ -0,0 +1,3 @@ +currentVelocity : 4.76814e+006 +desiredVelocity : 13.1119 +error : -4.76814e+006 diff --git a/Autopilot/models/autopilot/resources/sample1results/execution4de.rwth.armin.modeling.autopilot.motion.calculatePidError.res b/Autopilot/models/autopilot/resources/sample1results/execution4de.rwth.armin.modeling.autopilot.motion.calculatePidError.res new file mode 100644 index 0000000000000000000000000000000000000000..15e41f0871b17fb784824b2bf18b534bbeac23a1 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample1results/execution4de.rwth.armin.modeling.autopilot.motion.calculatePidError.res @@ -0,0 +1,3 @@ +currentVelocity : 8.31101e+006 +desiredVelocity : 7.40224 +error : -8.31101e+006 diff --git a/Autopilot/models/autopilot/resources/sample1results/execution5de.rwth.armin.modeling.autopilot.motion.calculatePidError.res b/Autopilot/models/autopilot/resources/sample1results/execution5de.rwth.armin.modeling.autopilot.motion.calculatePidError.res new file mode 100644 index 0000000000000000000000000000000000000000..97eeffcef0ec4961eb64f6dc2fc95153e9b93194 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample1results/execution5de.rwth.armin.modeling.autopilot.motion.calculatePidError.res @@ -0,0 +1,3 @@ +currentVelocity : 9.12446e+006 +desiredVelocity : 1.51394 +error : -9.12446e+006 diff --git a/Autopilot/models/autopilot/resources/sample1results/execution6de.rwth.armin.modeling.autopilot.motion.calculatePidError.res b/Autopilot/models/autopilot/resources/sample1results/execution6de.rwth.armin.modeling.autopilot.motion.calculatePidError.res new file mode 100644 index 0000000000000000000000000000000000000000..f5714a9de3637d938e55c8af41836b4c9167f678 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample1results/execution6de.rwth.armin.modeling.autopilot.motion.calculatePidError.res @@ -0,0 +1,3 @@ +currentVelocity : 6.65679e+006 +desiredVelocity : 9.68353 +error : -6.65679e+006 diff --git a/Autopilot/models/autopilot/resources/sample1results/execution7de.rwth.armin.modeling.autopilot.motion.calculatePidError.res b/Autopilot/models/autopilot/resources/sample1results/execution7de.rwth.armin.modeling.autopilot.motion.calculatePidError.res new file mode 100644 index 0000000000000000000000000000000000000000..78b1e60991d5d3c3ab92a971f1142c4dc8f37428 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample1results/execution7de.rwth.armin.modeling.autopilot.motion.calculatePidError.res @@ -0,0 +1,3 @@ +currentVelocity : 4.86116e+006 +desiredVelocity : 11.7957 +error : -4.86116e+006 diff --git a/Autopilot/models/autopilot/resources/sample1results/execution8de.rwth.armin.modeling.autopilot.motion.calculatePidError.res b/Autopilot/models/autopilot/resources/sample1results/execution8de.rwth.armin.modeling.autopilot.motion.calculatePidError.res new file mode 100644 index 0000000000000000000000000000000000000000..3dba5bbf87ec8ecb795cfd39b7a521f7a7f222bb --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample1results/execution8de.rwth.armin.modeling.autopilot.motion.calculatePidError.res @@ -0,0 +1,3 @@ +currentVelocity : 2.8876e+006 +desiredVelocity : 7.70555 +error : -2.8876e+006 diff --git a/Autopilot/models/autopilot/resources/sample1results/execution9de.rwth.armin.modeling.autopilot.motion.calculatePidError.res b/Autopilot/models/autopilot/resources/sample1results/execution9de.rwth.armin.modeling.autopilot.motion.calculatePidError.res new file mode 100644 index 0000000000000000000000000000000000000000..8ba850a0b07682c1bf48410102e70e63fbea975b --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample1results/execution9de.rwth.armin.modeling.autopilot.motion.calculatePidError.res @@ -0,0 +1,3 @@ +currentVelocity : 5.96108e+006 +desiredVelocity : 8.35107 +error : -5.96108e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.abs1.res b/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.abs1.res new file mode 100644 index 0000000000000000000000000000000000000000..808503e926b8890670b07dc1bd7cb957b26574a7 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.abs1.res @@ -0,0 +1,2 @@ +input : 8.05306e+006 +output : 8.05306e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..bd8c07dea28ac0d441572c4edd0a536d8f7da50d --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res @@ -0,0 +1,4 @@ +currentVelocity : 4.04329e+006 +desiredVelocity : -5.93673e+006 +engine : 0 +brakes : 176.703 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..2ae800441fe52582132bbd3dc5e2d8b2cf649435 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res @@ -0,0 +1,4 @@ +error : 1 +controlSignal : 5 +engine : 5 +brakes : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.pid.res b/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.pid.res new file mode 100644 index 0000000000000000000000000000000000000000..833c628c2a5a67586f8dc28e92283c3964999d5d --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.pid.res @@ -0,0 +1,6 @@ +paramP : -3.35052e+006 +paramI : 8.24641e+006 +paramD : 7.73813e+006 +paramDecayCoefficient : -1.96974e+006 +error : -2.09004e+006 +control : -1.02326e+013 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.pidError.res b/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.pidError.res new file mode 100644 index 0000000000000000000000000000000000000000..68e96fa091af5c9745e61a3a55183b789ba9c70d --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.pidError.res @@ -0,0 +1,3 @@ +currentVelocity : 6.69995e+006 +desiredVelocity : 8.53042 +error : -6.69995e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.pidParams.res b/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.pidParams.res new file mode 100644 index 0000000000000000000000000000000000000000..b91c01e43ce2397d5836cbb8fbfb3b147737cf31 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution0de.rwth.armin.modeling.autopilot.motion.pidParams.res @@ -0,0 +1,6 @@ +currentVelocity : 0 +desiredVelocity : 0 +paramP : 1.76703 +paramI : 0 +paramD : 0 +paramDecayCoefficient : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.abs1.res b/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.abs1.res new file mode 100644 index 0000000000000000000000000000000000000000..3fe2a6ebb6751aaa3f5a7a38fcd9acf9b09c3367 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.abs1.res @@ -0,0 +1,2 @@ +input : 5.81082e+006 +output : 5.81082e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..c623faaf5e2413542a5e2efc242f7ef8822efc21 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res @@ -0,0 +1,4 @@ +currentVelocity : -6.44653e+006 +desiredVelocity : -7.005e+006 +engine : 0 +brakes : 176.703 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..862224300e1ba88a5396b56c27c3cef9a3683580 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res @@ -0,0 +1,4 @@ +error : -10 +controlSignal : 20 +engine : 0 +brakes : 20 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.pid.res b/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.pid.res new file mode 100644 index 0000000000000000000000000000000000000000..84f5b0af84f96f4730c391e71568121bd731af4d --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.pid.res @@ -0,0 +1,6 @@ +paramP : -4.59075e+006 +paramI : -5.56741e+006 +paramD : -9.08603e+006 +paramDecayCoefficient : 3.33305e+006 +error : -918677 +control : 3.87838e+019 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.pidError.res b/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.pidError.res new file mode 100644 index 0000000000000000000000000000000000000000..ca036d4eed7f08ec80632f5d261d87a6f4c293d0 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.pidError.res @@ -0,0 +1,3 @@ +currentVelocity : 411381 +desiredVelocity : 13.5963 +error : -411381 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.pidParams.res b/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.pidParams.res new file mode 100644 index 0000000000000000000000000000000000000000..a5ac1ab91c3462bfb881ef77aa00a4e1230eb9d3 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution1de.rwth.armin.modeling.autopilot.motion.pidParams.res @@ -0,0 +1,6 @@ +currentVelocity : 1 +desiredVelocity : 2 +paramP : 1.82843 +paramI : 0 +paramD : 0 +paramDecayCoefficient : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.abs1.res b/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.abs1.res new file mode 100644 index 0000000000000000000000000000000000000000..55217ac5717823d092c4648d52948d821af7e38f --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.abs1.res @@ -0,0 +1,2 @@ +input : 6.44525e+006 +output : 6.44525e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..b2e08876b76754268f5b95c6649eeeefa2ee8324 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res @@ -0,0 +1,4 @@ +currentVelocity : 595598 +desiredVelocity : 5.92901e+006 +engine : 1.95407e+007 +brakes : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..fb969effdfd2c43581114e2694e8220db50a1267 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res @@ -0,0 +1,4 @@ +error : 4.25462e+006 +controlSignal : -1.37358e+006 +engine : -1.37358e+006 +brakes : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.pid.res b/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.pid.res new file mode 100644 index 0000000000000000000000000000000000000000..b6a01241ad8950d4784c3d118935a121e6ccc11c --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.pid.res @@ -0,0 +1,6 @@ +paramP : -4.63895e+006 +paramI : 654910 +paramD : -5.37126e+006 +paramDecayCoefficient : 1.03877e+006 +error : 7.29837e+006 +control : -4.73912e+024 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.pidError.res b/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.pidError.res new file mode 100644 index 0000000000000000000000000000000000000000..a531d5af437b46e00e33d54e4a58d5e7fe7ba2e4 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.pidError.res @@ -0,0 +1,3 @@ +currentVelocity : 3.90961e+006 +desiredVelocity : 8.74232 +error : -3.90961e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.pidParams.res b/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.pidParams.res new file mode 100644 index 0000000000000000000000000000000000000000..c3428e75d6b063b5fa7cc6800a1ab4f1d9454b50 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution2de.rwth.armin.modeling.autopilot.motion.pidParams.res @@ -0,0 +1,6 @@ +currentVelocity : 3 +desiredVelocity : 7 +paramP : 2.07401 +paramI : 0 +paramD : 0 +paramDecayCoefficient : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.abs1.res b/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.abs1.res new file mode 100644 index 0000000000000000000000000000000000000000..0b6bff6318ff0e3dddecfd21a1713c775d7ef27a --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.abs1.res @@ -0,0 +1,2 @@ +input : -258459 +output : 258459 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..3656afdda8c79d4a2574f38a9ac097c20911f459 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res @@ -0,0 +1,4 @@ +currentVelocity : 7.76738e+006 +desiredVelocity : -2.4035e+006 +engine : 0 +brakes : 176.703 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..379359194824eb9eaa975d0b01050bdd6efe4fcc --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res @@ -0,0 +1,4 @@ +error : 2.22074e+006 +controlSignal : -3.59068e+006 +engine : -3.59068e+006 +brakes : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.pid.res b/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.pid.res new file mode 100644 index 0000000000000000000000000000000000000000..f3aa52c648aa5d51fa8998223648f109f6e8d3c2 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.pid.res @@ -0,0 +1,6 @@ +paramP : -5.69942e+006 +paramI : 6.98268e+006 +paramD : 1.90818e+006 +paramDecayCoefficient : -5.61925e+006 +error : -9.7902e+006 +control : 2.83933e+032 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.pidError.res b/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.pidError.res new file mode 100644 index 0000000000000000000000000000000000000000..9301f1fb91b33f13b180d36a9d493656ddac5bf7 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.pidError.res @@ -0,0 +1,3 @@ +currentVelocity : 7.03924e+006 +desiredVelocity : 11.7836 +error : -7.03924e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.pidParams.res b/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.pidParams.res new file mode 100644 index 0000000000000000000000000000000000000000..24f8d0f7250ff04494c02afc547ba56d85d682aa --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution3de.rwth.armin.modeling.autopilot.motion.pidParams.res @@ -0,0 +1,6 @@ +currentVelocity : 13 +desiredVelocity : 13 +paramP : 3.29578 +paramI : 0 +paramD : 0 +paramDecayCoefficient : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.abs1.res b/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.abs1.res new file mode 100644 index 0000000000000000000000000000000000000000..ef08177e6c625290f4d874cdde073ba54d8936e0 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.abs1.res @@ -0,0 +1,2 @@ +input : 3.89685e+006 +output : 3.89685e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..75ab82f00efbcb27c638485e547fc25c64deb221 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res @@ -0,0 +1,4 @@ +currentVelocity : 8.24659e+006 +desiredVelocity : 9.28597e+006 +engine : 3.06045e+007 +brakes : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..739f40566166cf9968e63449e26c1dee443d0946 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res @@ -0,0 +1,4 @@ +error : -3.39001e+006 +controlSignal : -4.1934e+006 +engine : 0 +brakes : -4.1934e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.pid.res b/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.pid.res new file mode 100644 index 0000000000000000000000000000000000000000..c7608a476d9292a564cfcb9f59af48b25c799e7f --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.pid.res @@ -0,0 +1,6 @@ +paramP : 30739.4 +paramI : -1.61555e+006 +paramD : 3.24033e+006 +paramDecayCoefficient : -4.00947e+006 +error : 6.56576e+006 +control : 2.63391e+038 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.pidError.res b/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.pidError.res new file mode 100644 index 0000000000000000000000000000000000000000..aedab47a926ecf791ba28323d63a14abe72054a0 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.pidError.res @@ -0,0 +1,3 @@ +currentVelocity : 6.15309e+006 +desiredVelocity : 7.15305 +error : -6.15309e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.pidParams.res b/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.pidParams.res new file mode 100644 index 0000000000000000000000000000000000000000..84d67b3e68f8ff2a6915d115b4dd152f4c8cb43f --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution4de.rwth.armin.modeling.autopilot.motion.pidParams.res @@ -0,0 +1,6 @@ +currentVelocity : 15 +desiredVelocity : 13 +paramP : 3.29578 +paramI : 0 +paramD : 0 +paramDecayCoefficient : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.abs1.res b/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.abs1.res new file mode 100644 index 0000000000000000000000000000000000000000..9c9733d6fc5693e880d90dac48bf629b9b9a0c65 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.abs1.res @@ -0,0 +1,2 @@ +input : 9.34532e+006 +output : 9.34532e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..32843c4f608011be08a7bd0eb4d2a19d62f585c1 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res @@ -0,0 +1,4 @@ +currentVelocity : -4.95765e+006 +desiredVelocity : 6.72109e+006 +engine : 1.18764e+007 +brakes : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..cf756ee8934c4615dad2ce203db2baa701ee0ceb --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res @@ -0,0 +1,4 @@ +error : 2.16914e+006 +controlSignal : -6.62e+006 +engine : -6.62e+006 +brakes : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.pid.res b/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.pid.res new file mode 100644 index 0000000000000000000000000000000000000000..90c657bd019fe34b1c9a49f311fe56de9dd95e18 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.pid.res @@ -0,0 +1,6 @@ +paramP : 481855 +paramI : -9.15448e+006 +paramD : -9.27013e+006 +paramDecayCoefficient : 8.372e+006 +error : 1.30752e+006 +control : 1.24952e+046 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.pidError.res b/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.pidError.res new file mode 100644 index 0000000000000000000000000000000000000000..022fda23df5069eb60b863a2386023d70ffb982c --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.pidError.res @@ -0,0 +1,3 @@ +currentVelocity : 117077 +desiredVelocity : 8.87125 +error : -117077 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.pidParams.res b/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.pidParams.res new file mode 100644 index 0000000000000000000000000000000000000000..7cea8f7243c463ea7b6422ee5628993955dd398d --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution5de.rwth.armin.modeling.autopilot.motion.pidParams.res @@ -0,0 +1,6 @@ +currentVelocity : 5.75901e+006 +desiredVelocity : -764648 +paramP : 1.76703 +paramI : 0 +paramD : 0 +paramDecayCoefficient : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.abs1.res b/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.abs1.res new file mode 100644 index 0000000000000000000000000000000000000000..bbb15aa385cdaaac8920498b555bd77fb57cd095 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.abs1.res @@ -0,0 +1,2 @@ +input : 1.74837e+006 +output : 1.74837e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..c9125a6f0d293e0623a39ae19939eba6a223f355 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res @@ -0,0 +1,4 @@ +currentVelocity : -63990.1 +desiredVelocity : 3.35791e+006 +engine : 5.93352e+006 +brakes : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..9ce4ab6d32c3701ddcd2110e82f4272cd92c531e --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res @@ -0,0 +1,4 @@ +error : 4.53191e+006 +controlSignal : -101048 +engine : -101048 +brakes : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.pid.res b/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.pid.res new file mode 100644 index 0000000000000000000000000000000000000000..7867aa68e60bf35d19be804b40f50f73bc6568c1 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.pid.res @@ -0,0 +1,6 @@ +paramP : 7.48348e+006 +paramI : 1.52867e+006 +paramD : -1.11234e+006 +paramDecayCoefficient : 2.41393e+006 +error : -9.16606e+006 +control : -5.03675e+051 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.pidError.res b/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.pidError.res new file mode 100644 index 0000000000000000000000000000000000000000..6ddf2731a6e4425bab8bd4a06bb74a4a7dc2fc0f --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.pidError.res @@ -0,0 +1,3 @@ +currentVelocity : 5.05809e+006 +desiredVelocity : 1.89506 +error : -5.05809e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.pidParams.res b/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.pidParams.res new file mode 100644 index 0000000000000000000000000000000000000000..e9e7b8f1972b5cfdb01e94e7310719f4ed01eb86 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution6de.rwth.armin.modeling.autopilot.motion.pidParams.res @@ -0,0 +1,6 @@ +currentVelocity : 1.96172e+006 +desiredVelocity : 3.5477e+006 +paramP : 3.29578 +paramI : 0 +paramD : 0 +paramDecayCoefficient : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.abs1.res b/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.abs1.res new file mode 100644 index 0000000000000000000000000000000000000000..875e27007c7a06927326a50e0a78460232675965 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.abs1.res @@ -0,0 +1,2 @@ +input : -4.69217e+006 +output : 4.69217e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..c1005a4492b0efbaee93078ee38544fdab026c74 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res @@ -0,0 +1,4 @@ +currentVelocity : 9.50143e+006 +desiredVelocity : 672766 +engine : 0 +brakes : 3.13146e+007 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..8dbbde572416786ca54eb870fa763134d7f61aae --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res @@ -0,0 +1,4 @@ +error : -2.51414e+006 +controlSignal : 3.35227e+006 +engine : 0 +brakes : 3.35227e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.pid.res b/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.pid.res new file mode 100644 index 0000000000000000000000000000000000000000..b10612aa6f0ab52fd7cde38cb548c1e3444bcb24 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.pid.res @@ -0,0 +1,6 @@ +paramP : 7.19293e+006 +paramI : 8.74442e+006 +paramD : -9.92078e+006 +paramDecayCoefficient : -2.99924e+006 +error : -2.39974e+006 +control : 8.64128e+058 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.pidError.res b/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.pidError.res new file mode 100644 index 0000000000000000000000000000000000000000..5d92209865009d68f4626dc9139151c8574518e1 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.pidError.res @@ -0,0 +1,3 @@ +currentVelocity : 5.90115e+006 +desiredVelocity : 2.23495 +error : -5.90115e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.pidParams.res b/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.pidParams.res new file mode 100644 index 0000000000000000000000000000000000000000..6ac5dd9a6666860277833e866081f9f1c10160f4 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution7de.rwth.armin.modeling.autopilot.motion.pidParams.res @@ -0,0 +1,6 @@ +currentVelocity : -5.97213e+006 +desiredVelocity : -5.06441e+006 +paramP : 1.76703 +paramI : 0 +paramD : 0 +paramDecayCoefficient : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.abs1.res b/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.abs1.res new file mode 100644 index 0000000000000000000000000000000000000000..d2c787271c381b014afba6945f9a0b2639cc4956 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.abs1.res @@ -0,0 +1,2 @@ +input : -8.88211e+006 +output : 8.88211e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..4ca30980d6a31670bf61bf05dba90478bcc91913 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res @@ -0,0 +1,4 @@ +currentVelocity : -3.45587e+006 +desiredVelocity : 3.11098e+006 +engine : 5.49719e+006 +brakes : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..6bfe2ba87ab818f3c67b1b3b6e9abada9a7e42e9 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res @@ -0,0 +1,4 @@ +error : -5.11055e+006 +controlSignal : 3.62736e+006 +engine : 0 +brakes : 3.62736e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.pid.res b/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.pid.res new file mode 100644 index 0000000000000000000000000000000000000000..0cf3aa6fd50d3a8cbf845cac621e9a195577e447 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.pid.res @@ -0,0 +1,6 @@ +paramP : -2.80741e+006 +paramI : 667766 +paramD : 2.69515e+006 +paramDecayCoefficient : 2.89164e+006 +error : -911139 +control : 1.90816e+064 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.pidError.res b/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.pidError.res new file mode 100644 index 0000000000000000000000000000000000000000..fe6db5f4e523b9fc0b98aa6b09beaa57312fc203 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.pidError.res @@ -0,0 +1,3 @@ +currentVelocity : 2.64321e+006 +desiredVelocity : 0.877046 +error : -2.64321e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.pidParams.res b/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.pidParams.res new file mode 100644 index 0000000000000000000000000000000000000000..e2218e6e2dac64e9f7c74ccd9890c2cf552fb019 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution8de.rwth.armin.modeling.autopilot.motion.pidParams.res @@ -0,0 +1,6 @@ +currentVelocity : 1.4273e+006 +desiredVelocity : -6.32744e+006 +paramP : 1.76703 +paramI : 0 +paramD : 0 +paramDecayCoefficient : 0 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.abs1.res b/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.abs1.res new file mode 100644 index 0000000000000000000000000000000000000000..037900e7c94892ac68ef54d0bf1bf3363dfe340b --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.abs1.res @@ -0,0 +1,2 @@ +input : -3.31704e+006 +output : 3.31704e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..d69d0eed0f22401c537da6fadc2889fdc54db921 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.calculateEngineAndBrakes.res @@ -0,0 +1,4 @@ +currentVelocity : -9.82428e+006 +desiredVelocity : -349935 +engine : 0 +brakes : 176.703 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res b/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res new file mode 100644 index 0000000000000000000000000000000000000000..74afcea44bdca01ee647d6584feec4c2d6211496 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.engineOrBrakes.res @@ -0,0 +1,4 @@ +error : -591248 +controlSignal : -159959 +engine : 0 +brakes : -159959 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.pid.res b/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.pid.res new file mode 100644 index 0000000000000000000000000000000000000000..cd7d413deff85a6fbfe08e9174a560b0099fcb3a --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.pid.res @@ -0,0 +1,6 @@ +paramP : -2.50901e+006 +paramI : 5.63446e+006 +paramD : -7.37288e+006 +paramDecayCoefficient : 9.63694e+006 +error : -8.1774e+006 +control : 1.55161e+072 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.pidError.res b/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.pidError.res new file mode 100644 index 0000000000000000000000000000000000000000..a14aed34dddf93aef073e8472c89afba23d00e02 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.pidError.res @@ -0,0 +1,3 @@ +currentVelocity : 3.8709e+006 +desiredVelocity : 9.38952 +error : -3.8709e+006 diff --git a/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.pidParams.res b/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.pidParams.res new file mode 100644 index 0000000000000000000000000000000000000000..f7a1395f71e51418345cc7345b487e05b291e247 --- /dev/null +++ b/Autopilot/models/autopilot/resources/sample2results/execution9de.rwth.armin.modeling.autopilot.motion.pidParams.res @@ -0,0 +1,6 @@ +currentVelocity : -26927.2 +desiredVelocity : -8.73851e+006 +paramP : 1.76703 +paramI : 0 +paramD : 0 +paramDecayCoefficient : 0 diff --git a/Autopilot/models/wrapper/CalculateWrappedValues.emam b/Autopilot/models/wrapper/CalculateWrappedValues.emam new file mode 100644 index 0000000000000000000000000000000000000000..d7a3f85f09805a01c7471f8abd21008944558618 --- /dev/null +++ b/Autopilot/models/wrapper/CalculateWrappedValues.emam @@ -0,0 +1,54 @@ +package wrapper; +import wrapper.structures.*; + +component CalculateWrappedValues{ + + // ports for CARLA to Autopilot: + + ports + + in Vehicle_Status vehicle_status, + in Quaternion orientation, + + out Q (0.0 m/s : 0.01 m/s : oo m/s) velocity, + out Q (0.0 : 0.001 : 2.5) throttle, + out Q (-0.785 : 0.001 : 0.785) steer, + out Q (0.0 : 0.001 : 3.0) brake, + + out Q (-oo m : 0.01 m : oo m) pos_x, + out Q (-oo m : 0.01 m : oo m) pos_y, + + out Q (-oo : 0.001 : oo) compass, + + out Q (0.0 s : 0.001 s : 1.0 s) timeIncrement; + + + implementation Math{ + //Wrapping CARLA output into Autopilot input: + + Ego_Vehicle tmp_vehicle = vehicle_status.control; + + //autopilot: engine actuation (0, 2.5) + throttle = (tmp_vehicle.throttle * 2.5); + + //autopilot: steering (-0.785, 0.785) + steer = (tmp_vehicle.steer * 0.785); + + //autopilot: brakes (0, 3.0) + brake = (tmp_vehicle.brake * 3.0); + + velocity = vehicle_status.velocity; + + //carla-ros-bridge's frequency is 100ms + timeIncrement = 0.1; + + orientation = vehicle_status.orientation; + + //get the cars current position + pos_x = orientation.x; + pos_y = orientation.y; + + //calculate the compass value (relative to the direction the car is facing) + compass = asin(orientation.z) * 2; + } +} diff --git a/Autopilot/models/wrapper/CalculateWrapperOutValues.emam b/Autopilot/models/wrapper/CalculateWrapperOutValues.emam new file mode 100644 index 0000000000000000000000000000000000000000..03765d332bff8ae4d0467380769f14547e8d50a6 --- /dev/null +++ b/Autopilot/models/wrapper/CalculateWrapperOutValues.emam @@ -0,0 +1,24 @@ +package wrapper; +import wrapper.structures.*; + +component CalculateWrapperOutValues{ + // ports for Autopilot to CARLA: + ports + in Q (0.0 : 0.001 : 2.5) calculated_throttle, + in Q (-0.785 : 0.001 : 0.785) calculated_steer, + in Q (0.0 : 0.001 : 3.0) calculated_brake, + out Ego_Vehicle vehicle_control; + + + implementation Math{ + //Wrapping Autopilot output into CARLA input: + //convert actuation commands from autopilot into commands for CARLA + vehicle_control.throttle = (calculated_throttle / 2.5); + vehicle_control.steer = (calculated_steer / 0.785); + vehicle_control.brake = (calculated_brake / 3.0); + vehicle_control.hand_brake = false; + vehicle_control.reverse = false; + vehicle_control.gear = 1; + vehicle_control.manual_gear_shift = false; + } +} \ No newline at end of file diff --git a/Autopilot/models/wrapper/Wrapper.emam b/Autopilot/models/wrapper/Wrapper.emam new file mode 100644 index 0000000000000000000000000000000000000000..f25c1107b43197e3c3b81790684afd091f35a3fc --- /dev/null +++ b/Autopilot/models/wrapper/Wrapper.emam @@ -0,0 +1,48 @@ +package wrapper; +import wrapper.structures.*; +import wrapper.CalculateWrappedValues; +import wrapper.CalculateWrappedOutValues; +import autopilot.de.rwth.armin.modeling.autopilot.Autopilot; + +component Wrapper{ + + ports in Vehicle_Status vehicle_status, + + 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, + + out Ego_Vehicle vehicle_control; + + + //Connections for CARLA to Autopilot: + + instance CalculateWrappedValues cwv; + instance Autopilot autopilot; + instance CalculateWrapperOutValues cwvOut; + + connect vehicle_status -> cwv.vehicle_status; + + connect cwv.timeIncrement -> autopilot.timeIncrement; + + connect cwv.throttle -> autopilot.currentEngine; + connect cwv.steer -> autopilot.currentSteering; + connect cwv.brake -> autopilot.currentBrakes; + + connect cwv.velocity -> autopilot.currentVelocity; + connect cwv.pos_x -> autopilot.x; + connect cwv.pos_y -> autopilot.y; + connect cwv.compass -> autopilot.compass; + + connect trajectory_x -> autopilot.trajectory_x; + connect trajectory_y -> autopilot.trajectory_y; + connect trajectory_length -> autopilot.trajectory_length; + + //Connections for Autopilot to CARLA: + + connect autopilot.engine -> cwvOut.calculated_throttle; + connect autopilot.steering -> cwvOut.calculated_steer; + connect autopilot.brakes -> cwvOut.calculated_brake; + + connect cwvOut.vehicle_control -> vehicle_control; +} diff --git a/Autopilot/models/wrapper/WrapperTest.stream b/Autopilot/models/wrapper/WrapperTest.stream new file mode 100644 index 0000000000000000000000000000000000000000..3828035e7f46e540aca7e8248818acbfc632d2fe --- /dev/null +++ b/Autopilot/models/wrapper/WrapperTest.stream @@ -0,0 +1,35 @@ +package wrapper; + +stream WrapperTest for Wrapper{ + // Input + vehicle_status.velocity: 0; + vehicle_status.acceleration: 0; + + orientation.x: -3.41; + orientation.y: -1.97; + orientation.z: -0.72; + orientation.w: 0.69; + + control.throttle: 0; + control.steer: 0; + control.brake: 0; + control.hand_brake: false; + control.reverse: false; + control.gear: 1; + control.manual_gear_shift: false; + + trajectory_length: 99; + trajectory_x: [231.27, 231.27, 231.25, 231.22, 231.2, 231.17, 231.15, 231.13, 231.1, 231.08, 231.05, 231.03, 231.0, 230.98, 230.95, 230.93, 230.91, 230.88, 230.86, 230.83, 230.81, 230.78, 230.76, 230.74, 230.71, 230.69, 230.66, 230.64, 230.61, 230.59, 230.57, 230.54, 230.52, 230.49, 230.47, 230.44, 230.42, 230.39, 230.39, 230.37, 230.34, 230.32, 230.27, 230.12, 229.86, 229.49, 229.04, 228.49, 227.84, 227.15, 226.41, 225.64, 224.86, 223.99, 222.99, 221.0, 221.0, 220.0, 219.0, 218.0, 217.0, 216.0, 215.0, 214.0, 213.0, 212.0, 211.0, 210.0, 209.0, 208.0, 207.0, 206.0, 205.0, 204.0, 203.0, 202.0, 201.0, 200.0, 199.0, 198.0, 197.0, 196.0, 195.0, 194.0, 193.0, 192.0, 191.0, 190.0, 189.0, 188.0, 187.0, 186.0, 185.0, 184.0, 183.0, 182.0, 181.0, 179.99, 179.99]; + trajectory_y: [-14.27, -14.27, -15.27, -16.26, -17.26, -18.26, -19.26, -20.26, -21.26, -22.26, -23.26, -24.26, -25.26, -26.26, -27.26, -28.26, -29.26, -30.26, -31.26, -32.26, -33.26, -34.26, -35.26, -36.26, -37.26, -38.26, -39.26, -40.26, -41.26, -42.26, -43.26, -44.26, -45.26, -46.26, -47.26, -48.26, -49.26, -50.41, -50.41, -51.41, -52.41, -53.41, -54.22, -54.93, -55.61, -56.24, -56.81, -57.31, -57.76, -58.14, -58.42, -58.62, -58.73, -58.75, -58.75, -58.76, -58.76, -58.76, -58.76, -58.77, -58.77, -58.77, -58.77, -58.78, -58.78, -58.78, -58.78, -58.79, -58.79, -58.79, -58.79, -58.8, -58.8, -58.8, -58.8, -58.81, -58.81, -58.81, -58.82, -58.82, -58.82, -58.82, -58.83, -58.83, -58.83, -58.83, -58.84, -58.84, -58.84, -58.84, -58.85, -58.85, -58.85, -58.85, -58.86, -58.86, -58.86, -58.86, -58.86]; + + // Output + // - means: all values are allowed + // 100 +/- 1 means: all values from 99 to 101 are allowed + vehicle_control.throttle: 0 +/- 1; + vehicle_control.steer: -; + vehicle_control.brake: -; + vehicle_control.hand_brake: false; + vehicle_control.reverse: false; + vehicle_control.gear: -; + vehicle_control.manual_gear_shift: false; +} diff --git a/Autopilot/models/wrapper/structures/Ego_Vehicle.struct b/Autopilot/models/wrapper/structures/Ego_Vehicle.struct new file mode 100644 index 0000000000000000000000000000000000000000..5e809cb72e2a47e12be8e0b8d8bdc2d3ffc5994b --- /dev/null +++ b/Autopilot/models/wrapper/structures/Ego_Vehicle.struct @@ -0,0 +1,13 @@ +/* (c) https://github.com/MontiCore/monticore */ +package wrapper.structures; +import wrapper.structures.*; + +struct Ego_Vehicle{ + Q throttle; + Q steer; + Q brake; + B hand_brake; + B reverse; + Z gear; + B manual_gear_shift; +} diff --git a/Autopilot/models/wrapper/structures/Point.struct b/Autopilot/models/wrapper/structures/Point.struct new file mode 100644 index 0000000000000000000000000000000000000000..5aa53c0058511375b5af0c87ee1fa093bc0065f9 --- /dev/null +++ b/Autopilot/models/wrapper/structures/Point.struct @@ -0,0 +1,8 @@ +package wrapper.structures; +import wrapper.structures.*; + +struct Point{ + Q x; + Q y; + Q z; +} diff --git a/Autopilot/models/wrapper/structures/Pose.struct b/Autopilot/models/wrapper/structures/Pose.struct new file mode 100644 index 0000000000000000000000000000000000000000..39225b48da79706b890270fcb5c6f7d9e0eb8e95 --- /dev/null +++ b/Autopilot/models/wrapper/structures/Pose.struct @@ -0,0 +1,7 @@ +package wrapper.structures; +import wrapper.structures.*; + +struct Pose{ + Point point; + Quaternion orientation; +} diff --git a/Autopilot/models/wrapper/structures/Quaternion.struct b/Autopilot/models/wrapper/structures/Quaternion.struct new file mode 100644 index 0000000000000000000000000000000000000000..0a1ec88ba28ff24939df59a12cab149a9b014395 --- /dev/null +++ b/Autopilot/models/wrapper/structures/Quaternion.struct @@ -0,0 +1,9 @@ +package wrapper.structures; +import wrapper.structures.*; + +struct Quaternion{ + Q x; + Q y; + Q z; + Q w; +} diff --git a/Autopilot/models/wrapper/structures/Vehicle_Status.struct b/Autopilot/models/wrapper/structures/Vehicle_Status.struct new file mode 100644 index 0000000000000000000000000000000000000000..86646b38a803f0b01ef86ebb2a13c8725b01f18d --- /dev/null +++ b/Autopilot/models/wrapper/structures/Vehicle_Status.struct @@ -0,0 +1,9 @@ +package wrapper.structures; +import wrapper.structures.*; + +struct Vehicle_Status{ + Q velocity; + Q acceleration; + Quaternion orientation; + Ego_Vehicle control; +} diff --git a/Carla2EMAM/pom.xml b/Autopilot/pom.xml similarity index 53% rename from Carla2EMAM/pom.xml rename to Autopilot/pom.xml index 1d3e82e9e88d29d027d4369273bbf6a2968e33cf..4a5b949a905aec1c205fa4f892141abc30acbbdb 100644 --- a/Carla2EMAM/pom.xml +++ b/Autopilot/pom.xml @@ -6,12 +6,12 @@ 4.0.0 de.monticore.lang.monticar.utilities - Carla2EMAM + Autopilot 1.0-SNAPSHOT de.monticore.lang.monticar.utilities - Carlacomponents-bumper + CarlaComponents 1.0-SNAPSHOT @@ -25,26 +25,38 @@ de.monticore.lang.monticar.utilities maven-streamtest - 0.0.6-SNAPSHOT + 0.0.11-SNAPSHOT - Carla2EMAM/models - Carla2EMAM/src/test/emam - true - Carla2EMAM/target/tmp + Autopilot/models + Autopilot/models + false + Autopilot/target/tmp MinGW - Carla2EMAM/target/middleware + Autopilot/target/middleware - test.collisionDetection + wrapper.wrapper + false - compilemiddleware + test + + false + + test + + streamtest-execute + + + + compilemiddleware - cpp - roscpp + cpp + roscpp + false test @@ -52,8 +64,14 @@ + + + de.monticore.lang.monticar + embedded-montiarc-math-roscpp-generator + 0.1.8-SNAPSHOT + + - diff --git a/Bumpbot/Bumpbot.json b/Bumpbot/Bumpbot.json new file mode 100644 index 0000000000000000000000000000000000000000..852262dbbdcce40f72fdc3a02218d92d6fd18b88 --- /dev/null +++ b/Bumpbot/Bumpbot.json @@ -0,0 +1,7 @@ +{ + "modelsDir": "Bumpbot/models/", + "outputDir": "Bumpbot/target/", + "rootModel": "bumper.bumpBot", + "generators": ["cpp", "roscpp"] +} + diff --git a/EMAM2Carla/models/Bump.tag b/Bumpbot/models/Bump.tag similarity index 79% rename from EMAM2Carla/models/Bump.tag rename to Bumpbot/models/Bump.tag index 7d50de3b61d74d57ca17fc1eb4f7b3efeeac48fa..99be6a4bfa5bf050d30d636aed3fe3f33cbb1f0d 100644 --- a/EMAM2Carla/models/Bump.tag +++ b/Bumpbot/models/Bump.tag @@ -1,9 +1,9 @@ /* (c) https://github.com/MontiCore/monticore */ -package test; +package bumper; conforms to de.monticore.lang.monticar.generator.roscpp.RosToEmamTagSchema; tags Bumb{ - tag bumpBot.test with RosConnection = {topic = (/carla/ego_vehicle/vehicle_control_cmd, carla_msgs/CarlaEgoVehicleControl)}; + tag bumpBot.vehicle_control with RosConnection = {topic = (/carla/ego_vehicle/vehicle_control_cmd, carla_msgs/CarlaEgoVehicleControl)}; tag bumpBot.orientation with RosConnection = {topic = (/carla/ego_vehicle/vehicle_status, carla_msgs/CarlaEgoVehicleStatus), msgField = orientation}; tag bumpBot.collision_normale with RosConnection = {topic = (/carla/ego_vehicle/collision, carla_msgs/CarlaCollisionEvent), msgField = normal_impulse}; tag bumpBot.collision_normale_out with RosConnection = {topic = (/carla/ego_vehicle/collision, carla_msgs/CarlaCollisionEvent), msgField = normal_impulse}; diff --git a/EMAM2Carla/models/test/BumpBot.emam b/Bumpbot/models/bumper/BumpBot.emam similarity index 53% rename from EMAM2Carla/models/test/BumpBot.emam rename to Bumpbot/models/bumper/BumpBot.emam index 8a5b91268e9c0dde77e3261610d37a5d0b34e5fd..1e938a82da706603891d6fc3110bc3c06eaef786 100644 --- a/EMAM2Carla/models/test/BumpBot.emam +++ b/Bumpbot/models/bumper/BumpBot.emam @@ -1,13 +1,15 @@ /* (c) https://github.com/MontiCore/monticore */ -package test; -import test.structures.*; +package bumper; +import bumper.structures.*; component BumpBot{ - ports out Ego_Vehicle test, - in Quaternion orientation, + ports in Quaternion orientation, in Vector3 collision_normale, - out Vector3 collision_normale_out; + + out Ego_Vehicle vehicle_control, + out Vector3 collision_normale_out; + implementation Math{ static B inited = false; static B skip = false; @@ -20,39 +22,45 @@ component BumpBot{ collision_normale_out.z = collision_normale.z; else inited = true; - collision_normale_out.z = -1; - end - test.throttle = 1.0; - test.brake = 0.0; - test.hand_brake = false; + collision_normale_out.z = -1; + + end + + vehicle_control.throttle = 1.0; + vehicle_control.brake = 0.0; + vehicle_control.hand_brake = false; - test.gear = 1; - test.manual_gear_shift = false; + vehicle_control.gear = 1; + vehicle_control.manual_gear_shift = false; + Q tmp = asin(orientation.z); B noCollisionHappened = abs(collision_normale.z+1) < 0.000001; + if noCollisionHappened - test.reverse = false; - test.steer= 0; - else - Q drivingDirX = (1-(abs(tmp)/(M_PI/4))); + vehicle_control.reverse = false; + vehicle_control.steer= 0; + else + Q drivingDirX = (1-(abs(tmp)/(M_PI/4))); Q drivingDirY = 2*tmp/(M_PI/4); Q collisionOrtogonalDirection = -drivingDirX * collision_normale.y + drivingDirY * -collision_normale.x; Q collisionDirection = drivingDirX * -collision_normale.x + drivingDirY * collision_normale.y; Q projectionFactor = collisionDirection / sqrt(drivingDirX * drivingDirX + drivingDirY * drivingDirY); B turnCompleted = (projectionFactor * drivingDirX * -collision_normale.x) + (projectionFactor * drivingDirY * collision_normale.y) < 0; - if collisionDirection < 0.0 || turnCompleted - test.reverse = false; - test.steer= 0; + + if collisionDirection < 0.0 || turnCompleted + vehicle_control.reverse = false; + vehicle_control.steer= 0; skip = true; else - test.reverse = true; - if collisionOrtogonalDirection > 0.0 - test.steer = 0.5; + vehicle_control.reverse = true; + + if collisionOrtogonalDirection > 0.0 + vehicle_control.steer = 0.5; else - test.steer = -0.5; + vehicle_control.steer = -0.5; end end - end - end + end + end } } diff --git a/Bumpbot/models/bumper/BumpBotTest.stream b/Bumpbot/models/bumper/BumpBotTest.stream new file mode 100644 index 0000000000000000000000000000000000000000..78c0f625e6bbb167ad48e8713871dff107ecb3ab --- /dev/null +++ b/Bumpbot/models/bumper/BumpBotTest.stream @@ -0,0 +1,29 @@ +package bumper; + +stream BumpBotTest for BumpBot{ + // Input + orientation.x: 3.41; + orientation.y: -1.97; + orientation.z: -0.72; + orientation.w: -0.69; + + collision_normale.x: 1.23; + collision_normale.y: -2.34; + collision_normale.z: 0.56; + + // Output + // - means: all values are allowed + // 100 +/- 1 means: all values from 99 to 101 are allowed + vehicle_control.throttle: 0 +/- 1; + vehicle_control.steer: -; + vehicle_control.brake: -; + vehicle_control.hand_brake: false; + vehicle_control.reverse: true; + vehicle_control.gear: -; + vehicle_control.manual_gear_shift: false; + + collision_normale_out.x: -; + collision_normale_out.y: -; + collision_normale_out.z: -; + +} diff --git a/EMAM2Carla/models/test/structures/Collision.struct b/Bumpbot/models/bumper/structures/Collision.struct similarity index 59% rename from EMAM2Carla/models/test/structures/Collision.struct rename to Bumpbot/models/bumper/structures/Collision.struct index fc325ebca84c58e973a442fb79d06d02f086a654..3acf289e1882de14475738acc478fd598680dff0 100644 --- a/EMAM2Carla/models/test/structures/Collision.struct +++ b/Bumpbot/models/bumper/structures/Collision.struct @@ -1,5 +1,5 @@ -package test.structures; -import test.structures.*; +package bumper.structures; +import bumper.structures.*; struct Collision{ Z other_actor_id; diff --git a/EMAM2Carla/models/test/structures/Ego_Vehicle.struct b/Bumpbot/models/bumper/structures/Ego_Vehicle.struct similarity index 85% rename from EMAM2Carla/models/test/structures/Ego_Vehicle.struct rename to Bumpbot/models/bumper/structures/Ego_Vehicle.struct index 3fafb6eb8b7aa25e92d6d4b2b1f539646b35cab6..38291dc722c87e2458386df9af9468cef84c6d52 100644 --- a/EMAM2Carla/models/test/structures/Ego_Vehicle.struct +++ b/Bumpbot/models/bumper/structures/Ego_Vehicle.struct @@ -1,5 +1,5 @@ /* (c) https://github.com/MontiCore/monticore */ -package test.structures; +package bumper.structures; struct Ego_Vehicle{ Q throttle; diff --git a/EMAM2Carla/models/test/structures/Quaternion.struct b/Bumpbot/models/bumper/structures/Quaternion.struct similarity index 63% rename from EMAM2Carla/models/test/structures/Quaternion.struct rename to Bumpbot/models/bumper/structures/Quaternion.struct index 502102cc0f2be13f13574d19a9e81dd9c788badc..71100e3cf3dccf856aba850a5c4fadb020f96900 100644 --- a/EMAM2Carla/models/test/structures/Quaternion.struct +++ b/Bumpbot/models/bumper/structures/Quaternion.struct @@ -1,4 +1,4 @@ -package test.structures; +package bumper.structures; struct Quaternion{ Q x; diff --git a/EMAM2Carla/models/test/structures/Vector3.struct b/Bumpbot/models/bumper/structures/Vector3.struct similarity index 58% rename from EMAM2Carla/models/test/structures/Vector3.struct rename to Bumpbot/models/bumper/structures/Vector3.struct index 6d0f5b600713d22d2abb1622678b32035d8e09c5..9c06fc5330e179f7bcb7382628a3af16c3d2b4f6 100644 --- a/EMAM2Carla/models/test/structures/Vector3.struct +++ b/Bumpbot/models/bumper/structures/Vector3.struct @@ -1,4 +1,4 @@ -package test.structures; +package bumper.structures; struct Vector3{ Q x; diff --git a/EMAM2Carla/models/test/structures/Vehicle_Status.struct b/Bumpbot/models/bumper/structures/Vehicle_Status.struct similarity index 71% rename from EMAM2Carla/models/test/structures/Vehicle_Status.struct rename to Bumpbot/models/bumper/structures/Vehicle_Status.struct index 78a659d5bbc784f1d377541e48ac5d2fe85e794e..ff28ab7b96c19d7b678ccd35de84396d90d3ca10 100644 --- a/EMAM2Carla/models/test/structures/Vehicle_Status.struct +++ b/Bumpbot/models/bumper/structures/Vehicle_Status.struct @@ -1,5 +1,5 @@ -package test.structures; -import test.structures.*; +package bumper.structures; +import bumper.structures.*; struct Vehicle_Status{ Q velocity; diff --git a/EMAM2Carla/pom.xml b/Bumpbot/pom.xml similarity index 68% rename from EMAM2Carla/pom.xml rename to Bumpbot/pom.xml index 2cc1ad9afdd6a6fb217fcc0d4e2b9c2d9904696f..16a5e2a502b8a7d43b21b0981c71feede8805e51 100644 --- a/EMAM2Carla/pom.xml +++ b/Bumpbot/pom.xml @@ -6,12 +6,12 @@ 4.0.0 de.monticore.lang.monticar.utilities - EMAM2Carla + Bumpbot 1.0-SNAPSHOT de.monticore.lang.monticar.utilities - Carlacomponents-bumper + CarlaComponents 1.0-SNAPSHOT @@ -25,21 +25,31 @@ de.monticore.lang.monticar.utilities maven-streamtest - 0.0.6-SNAPSHOT + 0.0.11-SNAPSHOT - EMAM2Carla/models - EMAM2Carla/src/test/emam - true - EMAM2Carla/target/tmp + Bumpbot/models + Bumpbot/models + false + Bumpbot/target/tmp MinGW - EMAM2Carla/target/middleware + Bumpbot/target/middleware - test.bumpBot + bumper.bumpBot - compilemiddleware + test + + false + + test + + streamtest-execute + + + + compilemiddleware cpp @@ -56,11 +66,10 @@ de.monticore.lang.monticar embedded-montiarc-math-roscpp-generator - 0.1.5-SNAPSHOT + 0.1.8-SNAPSHOT - diff --git a/Carla2EMAM/models/CollisionDetection.tag b/Carla2EMAM/models/CollisionDetection.tag deleted file mode 100644 index 168bede44e096ed49f960409c0daab6c892908b7..0000000000000000000000000000000000000000 --- a/Carla2EMAM/models/CollisionDetection.tag +++ /dev/null @@ -1,9 +0,0 @@ -/* (c) https://github.com/MontiCore/monticore */ -package test; -conforms to de.monticore.lang.monticar.generator.roscpp.RosToEmamTagSchema; - -tags Bumb{ - tag collisionDetection.other_actor_id with RosConnection = {topic = (/carla/ego_vehicle/collision, carla_msgs/CarlaCollisionEvent), msgField = other_actor_id}; - tag collisionDetection.stdout with RosConnection = {topic = (/echo, std_msgs/Int64), msgField = data}; -} - diff --git a/Carla2EMAM/models/test/CollisionDetection.emam b/Carla2EMAM/models/test/CollisionDetection.emam deleted file mode 100644 index b422cc39e42c560ada405c6451daaabd2dff79f6..0000000000000000000000000000000000000000 --- a/Carla2EMAM/models/test/CollisionDetection.emam +++ /dev/null @@ -1,10 +0,0 @@ -/* (c) https://github.com/MontiCore/monticore */ -package test; - -component CollisionDetection{ - - port in Z other_actor_id, - out Z stdout; - - connect other_actor_id -> stdout; -} diff --git a/Carla2EMAM/src/test/emam/.keep b/Carla2EMAM/src/test/emam/.keep deleted file mode 100644 index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000 diff --git a/Carla2EMAM/valid.json b/Carla2EMAM/valid.json deleted file mode 100644 index 2825e819299da25debb8335e1c9346c386de1d36..0000000000000000000000000000000000000000 --- a/Carla2EMAM/valid.json +++ /dev/null @@ -1,7 +0,0 @@ -{ - "modelsDir": "models/", - "outputDir": "target/", - "rootModel": "test.collisionDetection", - "generators": ["cpp", "roscpp"] -} - diff --git a/EMAM2Carla/src/test/emam/.keep b/EMAM2Carla/src/test/emam/.keep deleted file mode 100644 index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000 diff --git a/EMAM2Carla/valid.json b/EMAM2Carla/valid.json deleted file mode 100644 index f382cf9fa56c238d082d03be7de157355c02cc4a..0000000000000000000000000000000000000000 --- a/EMAM2Carla/valid.json +++ /dev/null @@ -1,7 +0,0 @@ -{ - "modelsDir": "models/", - "outputDir": "target/", - "rootModel": "test.bumpBot", - "generators": ["cpp", "roscpp"] -} - diff --git a/HowToInstall.md b/HowToInstall.md index db56e8f8b27ec35029eb11d2c16a4938e8f5ad72..ab2ce88f63b9b4086621501a5b28867e0da6c494 100644 --- a/HowToInstall.md +++ b/HowToInstall.md @@ -1,27 +1,36 @@ # How to install everything -- Download Carla (We are using the precompiled version 0.9.5, because this version is currently supported by the ros-bridge) from `https://github.com/carla-simulator/carla/releases/tag/0.9.5` +- Download Carla (We are using the precompiled version 0.9.5, because this is the latest version that was supported by the ros-bridge at the time) from `https://github.com/carla-simulator/carla/releases/tag/0.9.5` - Extract it - If the file carla-0.9.5-py2.7-linux-x86_64.egg is not present inside the PythonAPI folder copy it there from PythonAPI/carla/dist - Install Docker -- If you want to build the CarlaRosBridge Docker Container yourself you can use the scripts in docker/carla-ros-bridge +- If you want to build the CarlaRosBridge Docker Container yourself you can use the scripts in docker/carla-ros-bridge; note that we use the tag 0.9.5.1 of the carla-ros-bridge as the latest version is not compatible with Carla 0.9.5 anymore. + You can edit your IP adress in the Dockerfile. -# How to run it on Linux -## How to execute the code for the bumper bot and read the collision sensor? +## Steps to build and execute a component: -- Start Carla by executing: `CarlaUE4.sh` -- Execute in PythonAPI/examples to create a vehicle for the ros-bridge: `python manual_control.py --rolename=ego_vehicle` -- To start the container execute inside the project folder: -`docker/bumpbot/run.sh` +1. Switch to the project you want to build (i.e: "carlacomponents") +2. Open shell, and execute: -Then execute in a new terminal to compile and execute the generated code for the bumpbot: -`docker/bumpbot/compile_exec.sh` + mvn clean install -s settings.xml + +3. If maven can't find carla_msgs message type: execute the following command and retry step 2: -If you want to read additional messages from a rostopic you can execute: -`docker/bumpbot/open_shell.sh` and inside the container read the topic with e.g. `rostopic echo /carla/ego_vehicle/collision` + export CMAKE_PREFIX_PATH=$CMAKE_PREFIX_PATH:/path/to/rosbridge/catkin_ws/devel/share/carla_msgs/cmake + + usually the path is something like `~/carla-ros-bridge/catkin_ws/devel/share/carla_msgs/cmake`. + +4. Switch to the target directory (of the component you want to run): -# Running with docker on Windows -I tested this code in a Command Prompt/Power Shell. For the use of other consoles the commands might need to be adjusted for example with winpty + cd /target/middleware/_/build/_/coordinator + +7. Execute Coordinator_\_\. Here it's: + + ./Coordinator_bumper_bumpBot + + or + + ./Coordinator_wrapper_wrapper ## Running the code - In the Docker Settings GUI select the drive where you want to work on as shared drive, else using volumes won't work. diff --git a/README.md b/README.md index 9e75bd2b43ac5ec626cf5e485d0a66349104941c..9104693ed0f866303239e142d93259dad17e556f 100644 --- a/README.md +++ b/README.md @@ -1,25 +1,54 @@ -Steps to build a component: - -* 1.) move **mw-generator.jar** to the directory of the component you wish to build. -* 2.) open shell, cd to the directory of the component and execute: **java -jar mw-generator.jar project.json** (in this case it's called **valid.json** for both components) -* 3.) switch to the target directory (**cd target/**) -* 4.) execute compile.sh (**./compile.sh**) -* 5.) if the generator can't find carla_msgs message type: execute the following command and retry step 4 -* export CMAKE_PREFIX_PATH=$CMAKE_PREFIX_PATH:/path/to/rosbridge/catkin_ws/devel/share/carla_msgs/cmake -* usually the path is something like "~/carla-ros-bridge/catkin_ws/devel/share/carla_msgs/cmake" -* 6.) after successfull compiling the generated code switch to install/bin directory (**cd install/bin/**) -* 7.) execute Coordinator__ (here it's **./Coordinator_test_bumpBot** or **./Coordinator_test_collisionDetection**) - -## Wie bringe ich das Auto zum Fahren und lese den Kollisionssensor aus? - -Carla Starten -Unter PythonAPI/examples: `python manual_control.py --rolename=ego_vehicle` -Zum Starten des Containers in den Ordner des Projektes (hier ein ausgechecktes Verzeichnis BumbBot_test) gehen: -`docker/run.sh` - -Dann in einem neuen Terminal um das Auto fahren zu lassen: -`docker/compile_exec.sh` - -Um den Collision Sensor auszulesen in einem neuen Terminal: -`docker/open_shell.sh` und darin `rostopic echo /carla/ego_vehicle/collision` +## Dependences +[Here](HowToInstall.md) is how to install everything (and how to run it manually). + +# How to move the Car by using ROS and reading information from the collision Sensor: +## How to run it on Linux + +1. Start the Carla-Simulator (`CarlaUE4.sh`) + +2. Start the ego-vehicle: + + docker/egovehicle.sh + + It executes `python manual_control.py --rolename=ego_vehicle` in PythonAPI/examples to create a vehicle for the ros-bridge. +3. To start the (docker-) container and carla-ros-bridge: + + docker/Linux/carla-ros-bridge.sh + +4. To move the vehicle by using ROS, open a new terminal and run: + + docker/Linux/bumpbot.sh + + This builds the bumpbot component and runs it. +5. You can now read information from the collision sensor by running: + + docker/Linux/open_shell.sh + + and within the shell: + + rostopic echo /carla/ego_vehicle/collision + +## Running with docker on Windows +I tested this code in a Command Prompt/Power Shell. For the use of other consoles the commands might need to be adjusted for example with winpty. + +Similarly to how you run it on Linux, there are shell scripts for Windows: carla-ros-bridge.sh, [component].sh and open_shell.sh. Depending on your system, these might need to be edited. +The scripts worked on the Setup: Windows 10 Home, Docker Toolbox, executed in git-bash. +To copy the assets into the docker container, I had to have the project folder in an shared-drive (you need to edit the carla-ros-bridge.sh). For me the output in `rostopic echo /carla/ego_vehicle/collision` was with great delay (~1 minute) or none at all, which just might be because of my slow system vs the Carla-simulator. + +I do not recommend trying to run everything on Windows. + +## Correlation with other projects: + +This project can be compiled as follows: + +1. with the EMAM2Middleware project directly as shown above +2. using the maven-streamtest plugin that also uses the EMAM2Middleware generator. + - run `mvn clean install -s settings.xml` + +EMAM2Middleware is build on top of EmbeddedMontiArc + +## Sample projects: + +[Bumpbot](Bumpbot) is a project where a bumperbot controls a car in the carla-simulator. +[Autopilot](Autopilot) contains code to reuse an autopilot for steering a car in carla. diff --git a/docker/Linux/autopilot.sh b/docker/Linux/autopilot.sh new file mode 100644 index 0000000000000000000000000000000000000000..7a8da6806d3bb51a7ce02d4e2684cc7bad418207 --- /dev/null +++ b/docker/Linux/autopilot.sh @@ -0,0 +1 @@ +docker exec -it carlacomponents /bin/bash -c 'source /opt/ros/kinetic/setup.bash && cd /usr/src/carlacomponents/ && rm -rf Autopilot/target && mvn clean install -pl Autopilot -s settings.xml && Autopilot/target/middleware/wrapper_wrapper/build/wrapper_wrapper/coordinator/Coordinator_wrapper_wrapper' diff --git a/docker/Linux/bumpbot.sh b/docker/Linux/bumpbot.sh new file mode 100644 index 0000000000000000000000000000000000000000..5c8d946cf6543442d6da90a321a5c87cc499d859 --- /dev/null +++ b/docker/Linux/bumpbot.sh @@ -0,0 +1,2 @@ +docker exec -it carlacomponents /bin/bash -c 'source /opt/ros/kinetic/setup.bash && cd /usr/src/carlacomponents/ && rm -rf Bumpbot/target && mvn clean install -pl Bumpbot -s settings.xml && Bumpbot/target/middleware/bumper_bumpBot/build/bumper_bumpBot/coordinator/Coordinator_bumper_bumpBot' + diff --git a/docker/Linux/carla-ros-bridge.sh b/docker/Linux/carla-ros-bridge.sh new file mode 100644 index 0000000000000000000000000000000000000000..444a883a5943b116569e7d32e076e92293408fee --- /dev/null +++ b/docker/Linux/carla-ros-bridge.sh @@ -0,0 +1 @@ +docker run -it --name carlacomponents --rm -v $(pwd):/usr/src/carlacomponents registry.git.rwth-aachen.de/monticore/embeddedmontiarc/applications/carlacomponents/emam-carla-ros-bridge diff --git a/docker/Linux/open_shell.sh b/docker/Linux/open_shell.sh new file mode 100644 index 0000000000000000000000000000000000000000..56d6d4ac30fb49a629d5631947fc12227c9b8d9f --- /dev/null +++ b/docker/Linux/open_shell.sh @@ -0,0 +1 @@ +docker exec -it carlacomponents /bin/bash -c 'source /opt/ros/kinetic/setup.bash && source opt/carla-ros-bridge/devel/setup.bash && /bin/bash' diff --git a/docker/Windows - docker Toolbox/autopilot.sh b/docker/Windows - docker Toolbox/autopilot.sh new file mode 100644 index 0000000000000000000000000000000000000000..aeeb08774c6228c8e4c569fa06cda55eff258ad2 --- /dev/null +++ b/docker/Windows - docker Toolbox/autopilot.sh @@ -0,0 +1,4 @@ +winpty docker exec \ +-it carlacomponents \ +bash \ +-c 'source /opt/ros/kinetic/setup.bash && cd /usr/src/carlacomponents/Autopilot/ && rm -rf target && cd .. && mvn clean install -pl Autopilot -s settings.xml && Autopilot/target/build/test_autopilot/coordinator/Coordinator_test_autopilot' diff --git a/docker/Windows - docker Toolbox/bumpbot.sh b/docker/Windows - docker Toolbox/bumpbot.sh new file mode 100644 index 0000000000000000000000000000000000000000..6354a07e1b5f74a77e88c2f8ee5ff98e15bb9782 --- /dev/null +++ b/docker/Windows - docker Toolbox/bumpbot.sh @@ -0,0 +1,4 @@ +winpty docker exec \ +-it carlacomponents \ +bash \ +-c 'source /opt/ros/kinetic/setup.bash && cd /usr/src/carlacomponents/Bumpbot/ && rm -rf target && cd .. && mvn clean install -pl Bumpbot -s settings.xml && Bumpbot/target/middleware/bumper_bumpBot/build/bumper_bumpBot/coordinator/Coordinator_bumper_bumpBot' diff --git a/docker/Windows - docker Toolbox/carla-ros-bridge.sh b/docker/Windows - docker Toolbox/carla-ros-bridge.sh new file mode 100644 index 0000000000000000000000000000000000000000..1acf9a914e9ecc843dd4d83d584e9afb9afad88d --- /dev/null +++ b/docker/Windows - docker Toolbox/carla-ros-bridge.sh @@ -0,0 +1,7 @@ +winpty docker run \ +-it \ +--rm \ +--name carlacomponents \ +--mount type=bind,source=//c/Users/Public/Documents/SharedFolder/carlacomponents,target=/usr/src/carlacomponents \ +registry.git.rwth-aachen.de/monticore/embeddedmontiarc/applications/carlacomponents/emam-carla-ros-bridge \ +#bash diff --git a/docker/win_open_shell.sh b/docker/Windows - docker Toolbox/open-shell.sh similarity index 85% rename from docker/win_open_shell.sh rename to docker/Windows - docker Toolbox/open-shell.sh index 41c23d5eea6c5b3e1a8eb9c18f2d1b8a04f160fc..84a66c552a43ae56a645b45ea9dbc7d6f6d6bd1f 100644 --- a/docker/win_open_shell.sh +++ b/docker/Windows - docker Toolbox/open-shell.sh @@ -1,4 +1,4 @@ winpty docker exec \ --it emam2carla \ +-it carlacomponents \ bash \ -c 'source /opt/ros/kinetic/setup.bash && source opt/carla-ros-bridge/devel/setup.bash && /bin/bash' diff --git a/docker/bumpbot/compile_exec.sh b/docker/bumpbot/compile_exec.sh deleted file mode 100644 index b4beef183940a3a5df075d7c39d0be4c3fb9d9c2..0000000000000000000000000000000000000000 --- a/docker/bumpbot/compile_exec.sh +++ /dev/null @@ -1,3 +0,0 @@ -docker exec -it emam2carla /bin/bash -c 'export PYTHONPATH=/opt/carla/PythonAPI/carla/dist/carla-0.9.5-py2.7-linux-x86_64.egg:/opt/carla/PythonAPI && source /opt/ros/kinetic/setup.bash && cd /usr/src/emam2carla/EMAM2Carla/ && rm -rf target && java -jar ../mw-generator.jar valid.json && target/compile.sh && target/build/test_bumpBot/coordinator/Coordinator_test_bumpBot' -# (c) https://github.com/MontiCore/monticore - diff --git a/docker/bumpbot/open_shell.sh b/docker/bumpbot/open_shell.sh deleted file mode 100644 index d753dd7dc816c38bad4747270c3bcf955f8636d4..0000000000000000000000000000000000000000 --- a/docker/bumpbot/open_shell.sh +++ /dev/null @@ -1,2 +0,0 @@ -docker exec -it emam2carla /bin/bash -c 'source /opt/ros/kinetic/setup.bash && source opt/carla-ros-bridge/devel/setup.bash && /bin/bash' -# (c) https://github.com/MontiCore/monticore diff --git a/docker/bumpbot/run.sh b/docker/bumpbot/run.sh deleted file mode 100644 index fba352ad9158691f3ad663458d79d8bde7cc6a98..0000000000000000000000000000000000000000 --- a/docker/bumpbot/run.sh +++ /dev/null @@ -1,2 +0,0 @@ -docker run -it --name emam2carla --rm -v $(pwd):/usr/src/emam2carla registry.git.rwth-aachen.de/monticore/embeddedmontiarc/applications/carlacomponents/emam-carla-ros-bridge -# (c) https://github.com/MontiCore/monticore diff --git a/docker/carla-ros-bridge/Dockerfile b/docker/carla-ros-bridge/Dockerfile index e10095102d359869671efdee35047536d1fa5712..fd4d5fc8696616b6e41d6bc81641a3272a54e6d3 100644 --- a/docker/carla-ros-bridge/Dockerfile +++ b/docker/carla-ros-bridge/Dockerfile @@ -5,16 +5,15 @@ FROM carlasim/carla:$CARLA_VERSION$CARLA_BUILD as carla FROM ros:kinetic-ros-base -RUN apt-get update \ - && apt-get install -y libpng16-16 ros-kinetic-ackermann-msgs ros-kinetic-derived-object-msgs ros-kinetic-tf ros-kinetic-cv-bridge --no-install-recommends \ - && rm -rf /var/lib/apt/lists/* +RUN apt-get update -qq \ + && apt-get -qq install -y --allow-unauthenticated libpng16-16 ros-kinetic-ackermann-msgs ros-kinetic-derived-object-msgs ros-kinetic-tf ros-kinetic-cv-bridge --no-install-recommends ARG CARLA_VERSION COPY --from=carla --chown=root /home/carla/PythonAPI /opt/carla/PythonAPI ENV PYTHONPATH=/opt/carla/PythonAPI/carla/dist/carla-$CARLA_VERSION-py2.7-linux-x86_64.egg:/opt/carla/PythonAPI -RUN git clone https://github.com/carla-simulator/ros-bridge.git /opt/carla-ros-bridge/git/ +RUN git clone --branch 0.9.5.1 https://github.com/carla-simulator/ros-bridge.git /opt/carla-ros-bridge/git RUN mkdir -p /opt/carla-ros-bridge/src RUN cp -r /opt/carla-ros-bridge/git/carla_ackermann_control /opt/carla-ros-bridge/src/carla_ackermann_control RUN cp -r /opt/carla-ros-bridge/git/carla_ros_bridge /opt/carla-ros-bridge/src/carla_ros_bridge @@ -22,14 +21,16 @@ RUN cp -r /opt/carla-ros-bridge/git/carla_manual_control /opt/carla-ros-bridge/s RUN cp -r /opt/carla-ros-bridge/git/carla_msgs /opt/carla-ros-bridge/src/carla_msgs RUN cp -r /opt/carla-ros-bridge/git/carla_ego_vehicle /opt/carla-ros-bridge/src/carla_ego_vehicle RUN cp -r /opt/carla-ros-bridge/git/carla_waypoint_publisher /opt/carla-ros-bridge/src/carla_waypoint_publisher + +# REPLACE WITH YOUR IP if not used on linux with default docker network +RUN sed -i 's/localhost/172.17.0.1/g' /opt/carla-ros-bridge/src/carla_ros_bridge/config/settings.yaml RUN /bin/bash -c 'source /opt/ros/kinetic/setup.bash; cd /opt/carla-ros-bridge; catkin_make -DCMAKE_BUILD_TYPE=Release install' # replace entrypoint -COPY ./docker/carla-ros-bridge/content/ros_entrypoint.sh / +RUN cp -r /opt/carla-ros-bridge/git/docker/content/ros_entrypoint.sh / # Install Emam & Java -RUN apt-get update \ - && apt-get install -y unzip g++ libopenblas-dev libhdf5-serial-dev cmake maven default-jdk --no-install-recommends \ +RUN apt-get install -y -qq --allow-unauthenticated unzip g++ libopenblas-dev libhdf5-serial-dev cmake maven default-jdk --no-install-recommends \ && rm -rf /var/lib/apt/lists/* ENV CMAKE_PREFIX_PATH="${CMAKE_PREFIX_PATH}:/opt/carla-ros-bridge/devel/share/carla_msgs/cmake" ENV PATH="/usr/lib/x86_64-linux-gnu/:${PATH}" diff --git a/docker/carla-ros-bridge/build.sh b/docker/carla-ros-bridge/build.sh index fcf8094e6a090a9f07bae676180e235f3adeb858..efa232aaf14a579f1a2047804c47e93b586060f6 100644 --- a/docker/carla-ros-bridge/build.sh +++ b/docker/carla-ros-bridge/build.sh @@ -1,4 +1,4 @@ #!/bin/sh # (c) https://github.com/MontiCore/monticore - -docker build -t carla-ros-bridge -f Dockerfile ./.. "$@" +curDir=$(readlink -f `dirname $0`) +docker build -t registry.git.rwth-aachen.de/monticore/embeddedmontiarc/applications/carlacomponents/emam-carla-ros-bridge:latest -f Dockerfile ./.. "$@" diff --git a/docker/carla-ros-bridge/build2.sh b/docker/carla-ros-bridge/build2.sh deleted file mode 100644 index 0f50df3892dec136a1fceca0bd3a85ae4b5a1999..0000000000000000000000000000000000000000 --- a/docker/carla-ros-bridge/build2.sh +++ /dev/null @@ -1,3 +0,0 @@ -curDir=$(readlink -f `dirname $0`) -# (c) https://github.com/MontiCore/monticore -docker build -t registry.git.rwth-aachen.de/monticore/embeddedmontiarc/applications/carlacomponents/emam-carla-ros-bridge:latest -f Dockerfile ./.. "$@" diff --git a/docker/egovehicle.sh b/docker/egovehicle.sh new file mode 100644 index 0000000000000000000000000000000000000000..723b983a7148c4dd5255b7566bb87cccdd598d13 --- /dev/null +++ b/docker/egovehicle.sh @@ -0,0 +1,2 @@ +cd /c//Programs/carla/PythonAPI/examples/ +python manual_control.py --rolename=ego_vehicle \ No newline at end of file diff --git a/docker/exec.sh b/docker/exec.sh deleted file mode 100644 index 6e2272b5fd1e35a9c71d1e739bd8b3d28bf09b93..0000000000000000000000000000000000000000 --- a/docker/exec.sh +++ /dev/null @@ -1,2 +0,0 @@ -docker exec -it emam2carla /bin/bash -c 'source /opt/ros/kinetic/setup.bash && cd /usr/src/emam2carla/EMAM2Carla/ && target/build/test_bumpBot/coordinator/Coordinator_test_bumpBot' - diff --git a/docker/win_compile_exec.ps b/docker/win_compile_exec.ps deleted file mode 100644 index 6b0b534ca71d1a65fa7217691aeee4cd96cb5867..0000000000000000000000000000000000000000 --- a/docker/win_compile_exec.ps +++ /dev/null @@ -1 +0,0 @@ -docker exec -it heuristic_wescoff bash -c 'source /opt/ros/kinetic/setup.bash && cd /usr/src/emam2carla/EMAM2Carla/ && rm -rf target && java -jar ../mw-generator.jar valid.json && target/compile.sh && target/build/test_bumpBot/coordinator/Coordinator_test_bumpBot' diff --git a/docker/win_compile_exec.sh b/docker/win_compile_exec.sh deleted file mode 100644 index e8cbac1731a18fa7445b05c55166fc33dcaece40..0000000000000000000000000000000000000000 --- a/docker/win_compile_exec.sh +++ /dev/null @@ -1 +0,0 @@ -docker exec -it heuristic_wescoff bash -c 'source /opt/ros/kinetic/setup.bash && cd /usr/src/emam2carla/EMAM2Carla/ && rm -rf target && java -jar ../mw-generator.jar valid.json && target/compile.sh && target/build/test_bumpBot/coordinator/Coordinator_test_bumpBot' diff --git a/docker/win_run.bat b/docker/win_run.bat deleted file mode 100644 index df4780107fa859ee1f26a123f521aee397646f3e..0000000000000000000000000000000000000000 --- a/docker/win_run.bat +++ /dev/null @@ -1 +0,0 @@ -docker run -it --rm --name emam2carla --mount type=volume,source=//h/Files/git/praktikum/bumpbot,target=/usr/src/emam2carla registry.git.rwth-aachen.de/monticore/embeddedmontiarc/applications/carlacomponents/emam-carla-ros-bridge bash diff --git a/docker/win_run.sh b/docker/win_run.sh deleted file mode 100644 index 20739f4638b7d7b60667ce064755ec3e2c8f2889..0000000000000000000000000000000000000000 --- a/docker/win_run.sh +++ /dev/null @@ -1 +0,0 @@ -docker run -it --rm --name emam2carla --mount volumem=bind,source=//h/Files/git/praktikum/bumpbot,target=/usr/src/emam2carla registry.git.rwth-aachen.de/monticore/embeddedmontiarc/applications/carlacomponents/emam-carla-ros-bridge bash diff --git a/mw-generator.jar b/mw-generator.jar deleted file mode 100644 index db4e0692384f7268449c88149f77932cde335071..0000000000000000000000000000000000000000 Binary files a/mw-generator.jar and /dev/null differ diff --git a/pom.xml b/pom.xml index ff5f75541cebda94ee10d3a81a4b89aca05dc490..6ca451765e68fde4ded641e7ab0f65f884838262 100644 --- a/pom.xml +++ b/pom.xml @@ -4,13 +4,13 @@ 4.0.0 de.monticore.lang.monticar.utilities - Carlacomponents-bumper + CarlaComponents 1.0-SNAPSHOT pom - Carla2EMAM - EMAM2Carla + Bumpbot + Autopilot