Merge branch 'CollisionExperiment' into 'master'

Add BumperBot to the master branch

See merge request !5

.gitlab-ci.yml

@@ -8,4 +8,3 @@ buildLinux:
         script:
                - mvn -Dorg.slf4j.simpleLogger.log.org.apache.maven.cli.transfer.Slf4jMavenTransferListener=warn -B clean install --settings settings.xml -U
 
-

EMAM2Carla/models/Bump.tag

@@ -4,5 +4,7 @@ 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.collision_actor_id with RosConnection = {topic = (/carla/ego_vehicle/collision, carla_msgs/CarlaCollisionEvent), msgField = other_actor_id};
+	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};
 }

EMAM2Carla/models/test/BumpBot.emam

@@ -4,19 +4,55 @@ import test.structures.*;
 
 component BumpBot{
 
-	port 	out Ego_Vehicle test,
-		in Z collision_actor_id;
-
+	ports	out Ego_Vehicle test,
+		in Quaternion orientation,
+		in Vector3 collision_normale,
+    out Vector3 collision_normale_out;
 	implementation Math{
-		test.throttle = 1.0;
-		test.brake = 0.0;
-		test.hand_brake = false;
-		test.reverse = false;
-		test.gear = 1;
-		test.manual_gear_shift = false;		
-
-		if collision_actor_id > 0 //if a collision occurred drive into the other direction
-			test.reverse = true;
+		static B inited = false;
+		static B skip = false;
+		if skip
+			skip = false;
+		else
+			if inited // Keep values!
+				collision_normale_out.x = collision_normale.x;
+				collision_normale_out.y = collision_normale.y;
+				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;
+			
+			test.gear = 1;
+			test.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)));
+				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;
+					skip = true;
+				else
+					test.reverse = true;
+		   		if collisionOrtogonalDirection > 0.0
+						test.steer = 0.5;
+					else
+						test.steer = -0.5;
+					end			
+				end
+		  end
 		end
 	}
 }

EMAM2Carla/models/test/structures/Collision.struct

+package test.structures;
+import test.structures.*;
+
+struct Collision{
+        Z other_actor_id;
+        Vector3 normal_impulse;
+}
+

EMAM2Carla/models/test/structures/Quaternion.struct

+package test.structures;
+
+struct Quaternion{
+	Q x;
+	Q y;
+	Q z;
+	Q w;
+}
+

EMAM2Carla/models/test/structures/Vector3.struct

+package test.structures;
+
+struct Vector3{
+	Q x;
+	Q y;
+	Q z;
+}
+

EMAM2Carla/models/test/structures/Vehicle_Status.struct

+package test.structures;
+import test.structures.*;
+
+struct Vehicle_Status{
+        Q velocity;
+        Q acceleration;
+        Quaternion orientation;
+        Ego_Vehicle ego_vehicle;
+}
+
+

EMAM2Carla/pom.xml

@@ -32,7 +32,7 @@
 	  <runStreamTestBefore>true</runStreamTestBefore>
           <pathTmpOut>EMAM2Carla/target/tmp</pathTmpOut>
           <generator>MinGW</generator>
-          <pathMiddlewareOut>Carla2EMAM/target/middleware</pathMiddlewareOut>
+          <pathMiddlewareOut>EMAM2Carla/target/middleware</pathMiddlewareOut>
           <middlewareRootModels>
               <param>test.bumpBot</param>
           </middlewareRootModels>
@@ -52,6 +52,13 @@
             </goals>
           </execution>
         </executions>
+        <dependencies>
+          <dependency>
+            <groupId>de.monticore.lang.monticar</groupId>
+            <artifactId>embedded-montiarc-math-roscpp-generator</artifactId>
+            <version>0.1.5-SNAPSHOT</version>
+          </dependency>
+        </dependencies>
       </plugin>
     </plugins>
   </build>

docker/exec.sh

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

docker/win_compile_exec.ps

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

docker/win_compile_exec.sh

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

docker/win_open_shell.sh

+winpty docker exec \
+-it emam2carla \
+bash \
+-c 'source /opt/ros/kinetic/setup.bash && source opt/carla-ros-bridge/devel/setup.bash && /bin/bash'

docker/win_run.bat

+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

docker/win_run.sh

+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