Commit 8d020b76 authored by Alexander David Hellwig's avatar Alexander David Hellwig

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

Bumper and autopilot

See merge request !13
parents 274f98ce 444fa9ab
Pipeline #188757 passed with stage
in 5 minutes and 33 seconds
target/
Bumpbot/target/
Autopilot/target/
Autopilot/models/.vscode
improved-array-handling-test/target/
{
"modelsDir": "Autopilot/models/",
"outputDir": "Autopilot/target/",
"rootModel": "wrapper.wrapper",
"generators": ["cpp", "roscpp"]
}
<!-- (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
#!/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
{
"configurations": [
{
"type": "emam",
"request": "launch",
"name": "Wrapper.emam",
"program": "wrapper.wrapper",
"stopOnEntry": true,
"modelBase": "${workspaceFolder}"
}
]
}
\ No newline at end of file
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};
}
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;
}
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;
}
}
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;
}
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
}
}
package autopilot.de.rwth.armin.modeling.autopilot.basicComp;
view PID {
component PID{
ports in ? currentSpeed,
in ? desiredSpeed,
out ? Engine,
out ? Brakes;
}
}
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;
}
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;
}
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;
}
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;
}
}
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;