Merge branch 'bumper-and-autopilot' into 'master'

Bumper and autopilot

See merge request !13

.gitignore

-target/
+Bumpbot/target/
+Autopilot/target/
+Autopilot/models/.vscode
+improved-array-handling-test/target/

Autopilot/Autopilot.json

+{
+  "modelsDir": "Autopilot/models/",
+  "outputDir": "Autopilot/target/",
+  "rootModel": "wrapper.wrapper",
+  "generators": ["cpp", "roscpp"]
+}
+

Autopilot/README.md

+<!-- (c) https://github.com/MontiCore/monticore -->
+## 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

Autopilot/converter.py

+#!/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

Autopilot/models/.vscode/launch.json

+{
+    "configurations": [
+        {
+            "type": "emam",
+            "request": "launch",
+            "name": "Wrapper.emam",
+            "program": "wrapper.wrapper",
+            "stopOnEntry": true,
+            "modelBase": "${workspaceFolder}"
+        }
+    ]
+}
\ No newline at end of file

Autopilot/models/Wrapper.tag

+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};
+}

Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/Autopilot.emam

+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;
+}

Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/Autopilot.emv

+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;
+	
+	}
+}

Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/Tests.emam

+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;
+}

Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/basicComp/PID.emam

+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
+ }
+
+}

Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/basicComp/PID.emv

+package autopilot.de.rwth.armin.modeling.autopilot.basicComp;
+
+
+view PID {
+	component PID{
+		ports in ? currentSpeed,
+			  in ? desiredSpeed,
+			  out ? Engine,
+			  out ? Brakes;
+	}
+}

Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/basicComp/PID1.stream

+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;
+}

Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/BehaviorGeneration.emam

+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;
+}

Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/CalculateMotionCommands.emam

+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;
+}

Autopilot/models/autopilot/de/rwth/armin/modeling/autopilot/behavior/Distance2Velocity.emam

+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;