feat(#106): added documentation-> this branch is ready for merge

.gitignore

@@ -11,4 +11,3 @@ code/output
 
 # Byte-compiled / optimized / DLL files
 __pycache__/
-

build/docker-compose.yml

@@ -57,8 +57,8 @@ services:
     tty: true
     shm_size: 2gb
     #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS"
-    #command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch"
-    command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP"
+    command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch"
+    #command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP"
     logging:
       driver: "local"
     environment:

code/acting/launch/acting.launch

@@ -29,10 +29,10 @@
         <param name="role_name" value="$(arg role_name)" />
     </node>
 
-    <!--node pkg="acting" type="Acting_DebuggerNode.py" name="Acting_Debugger" output="screen">
+    <node pkg="acting" type="Acting_DebuggerNode.py" name="Acting_Debugger" output="screen">
         <param name="control_loop_rate" value="0.05" />
         <param name="role_name" value="$(arg role_name)" />
-    </node-->
+    </node>
 
     <node pkg="acting" type="MainFramePublisher.py" name="MainFramePublisher" output="screen">
         <param name="control_loop_rate" value="0.05" />

code/perception/launch/perception.launch

@@ -31,6 +31,7 @@
         Heading Filter values:
         - "Kalman" (Default)
         - "None"
+        - "Old" (Buggy for demonstration purposes only)
       -->
   </node>
 

code/perception/src/Position_Publisher_Node.py

@@ -3,7 +3,8 @@
 """
 This node publishes all relevant topics for the ekf node.
 """
-# import math
+import math
+from tf.transformations import euler_from_quaternion
 import numpy as np
 import ros_compatibility as roscomp
 from ros_compatibility.node import CompatibleNode
@@ -104,7 +105,9 @@ def __init__(self):
             # No additional subscriber needed since the unfiltered heading
             # data is subscribed by self.imu_subscriber
             pass
+
     # endregion Subscriber END
+
     # region Publisher START
         # IMU
         # self.ekf_imu_publisher = self.new_publisher(
@@ -186,6 +189,19 @@ def publish_unfiltered_heading(self, data: Imu):
         if self.heading_filter == "None":
             self.__heading = heading
             self.__heading_publisher.publish(self.__heading)
+        elif self.heading_filter == "Old":
+            # In the case of using "Old" filter, the heading is
+            # calculated the WRONG way, just for demonstration purposes
+            roll, pitch, yaw = euler_from_quaternion(data_orientation_q)
+            raw_heading = math.atan2(roll, pitch)
+
+            # transform raw_heading so that:
+            # ---------------------------------------------------------------
+            # | 0 = x-axis | pi/2 = y-axis | pi = -x-axis | -pi/2 = -y-axis |
+            # ---------------------------------------------------------------
+            heading = (raw_heading - (math.pi / 2)) % (2 * math.pi) - math.pi
+            self.__heading = heading
+            self.__heading_publisher.publish(self.__heading)
         else:
             # in each other case the heading is published as unfiltered heading
             # for further filtering in other nodes such as the kalman node

code/perception/src/kalman_filter.py

@@ -46,7 +46,6 @@
                     [0, 0, 0, 1, 0, 0],
                     [0, 0, 0, 0, 1, 0],
                     [0, 0, 0, 0, 0, 1]])
-    I: Identity Matrix
 The measurement matrix H is defined as:
     self.H = np.array([[1, 0, 0, 0, 0, 0],    # x
                         [0, 1, 0, 0, 0, 0],   # y
@@ -168,7 +167,7 @@ def __init__(self):
         # Initialize the subscriber for the IMU Data
         self.imu_subscriber = self.new_subscription(
             Imu,
-            "/carla/" + self.role_name + "/Ideal_IMU",
+            "/carla/" + self.role_name + "/IMU",
             self.update_imu_data,
             qos_profile=1)
         # Initialize the subscriber for the GPS Data
@@ -177,13 +176,13 @@ def __init__(self):
             "/carla/" + self.role_name + "/GPS",
             self.update_gps_data,
             qos_profile=1)
-        # Initialize the subscriber for the current_pos in XYZ
+        # Initialize the subscriber for the unfiltered_pos in XYZ
         self.avg_z = np.zeros((GPS_RUNNING_AVG_ARGS, 1))
         self.avg_gps_counter: int = 0
         self.unfiltered_pos_subscriber = self.new_subscription(
             PoseStamped,
             "/paf/" + self.role_name + "/unfiltered_pos",
-            self.update_current_pos,
+            self.update_unfiltered_pos,
             qos_profile=1)
         # Initialize the subscriber for the velocity
         self.velocity_subscriber = self.new_subscription(
@@ -209,7 +208,7 @@ def run(self):
         Run the Kalman Filter
         """
         # wait until the car receives the first GPS Data
-        # from the update_current_pos method
+        # from the update_unfiltered_pos method
         while not self.initialized:
             rospy.sleep(1)
         rospy.sleep(1)
@@ -249,7 +248,7 @@ def predict(self):
         """
         # Predict the next state and covariance matrix, pretending the last
         # velocity state estimate stayed constant
-        self.x_pred = self.A @ self.x_est  # + B @ v[:] + u
+        self.x_pred = self.A @ self.x_est 
         self.P_pred = self.A @ self.P_est @ self.A.T + self.Q
 
     def update(self):
@@ -357,17 +356,17 @@ def update_gps_data(self, gps_data):
         """
         pass
 
-    def update_current_pos(self, current_pos):
+    def update_unfiltered_pos(self, unfiltered_pos):
         """
         Update the current position
         ALSO: allows the kalman filter to start running
               by setting self.initialized to True
         """
         # update GPS Measurements:
-        self.z_gps[0, 0] = current_pos.pose.position.x
-        self.z_gps[1, 0] = current_pos.pose.position.y
+        self.z_gps[0, 0] = unfiltered_pos.pose.position.x
+        self.z_gps[1, 0] = unfiltered_pos.pose.position.y
 
-        z = current_pos.pose.position.z
+        z = unfiltered_pos.pose.position.z
 
         self.avg_z = np.roll(self.avg_z, -1, axis=0)
         self.avg_z[-1] = np.matrix([z])

doc/06_perception/00_coordinate_transformation.md

@@ -1,6 +1,6 @@
 # Coordinate Transformation
 
-**Summary:** Used for various helper functions such as quat_to_heading, that are useful in a lot of cases.
+**Summary:** Used for various helper functions such as quat_to_heading, that are useful in a lot of cases. **It is not yet fully documented**.
 
 ---
 
@@ -14,14 +14,132 @@ Robert Fischer
 
 <!-- TOC -->
 
-- [Coordinate Transformation](#dataset-generator)
+- [Coordinate Transformation](#coordonate-transformation)
   - [Author](#author)
   - [Date](#date)
-  - [Necessary adjustments](#necessary-adjustments)
   - [Usage](#usage)
+  - [Methods](#methods)
+    - [quat_to_heading(quaternion)](#quat_to_headingquaternion)
 
 <!-- TOC -->
 
-## Necessary adjustments
-
 ## Usage
+
+Just importing the coordinate_transformation.py file is enough to use all of its funcions.
+
+```Python
+# Example
+from coordinate_transformation import quat_to_heading
+```
+
+## Methods
+
+This class provides multiple useful methods:
+
+### quat_to_heading(quaternion)
+
+For the cars orientation we need the angle of the cars Heading around the **z-axis**.
+We are provided with a Quaternion of the cars Rotation with respect to the Global Coordinate System.
+
+Also the car always starts in the direction of the `x-axis`, which is what our `0 rad point` will be.
+
+To calculate the Heading we first need to create a Rotation Matrix out of the quaternion:
+
+`R = Rotation.from_quat(quaternion).rotation.as_matrix()`
+
+$$
+R =
+\begin{bmatrix}
+    a & b & c\\
+    d & e & f\\
+    g & h & i\\
+\end{bmatrix}
+$$
+
+The original vector `V` of the car is `(1,0,0)`, since it spawns looking into the x-axis direction.
+
+$$
+V =
+\begin{bmatrix}
+    1\\
+    0\\
+    0\\
+\end{bmatrix}
+$$
+
+To calculate our rotated vector `V'` we do the following:
+
+$$
+
+V' = R \cdot V
+
+\\
+
+\begin{bmatrix}
+    a\\
+    d\\
+    g\\
+\end{bmatrix}
+
+=
+\begin{bmatrix}
+    a & b & c\\
+    d & e & f\\
+    g & h & i\\
+\end{bmatrix}
+\cdot
+\begin{bmatrix}
+    1\\
+    0\\
+    0\\
+\end{bmatrix}
+$$
+
+So we end up with a vector that's rotated into the x-y plane with the new x and y coordinates being `a` and `d`:
+
+![quat_to_angle](../../doc/00_assets/perception/quat_to_angle.png)
+
+Now all we need to do is calculate the angle $\theta$ around the z-axis which this vector creates between the x-axis and itself using the `atan` function:
+
+$$
+\theta = atan2(\frac{d}{a})
+$$
+
+To finish up we rotate the heading angle around 90 degrees, so the x-axis is our 0 rad point:
+
+`heading = (raw_heading - (math.pi / 2)) % (2 * math.pi) - math.pi`
+
+Now the Heading is 0 rad in the x-axis start position and `positive when rotating counter clockwise`.
+
+```Python
+
+    """
+    Converts a quaternion to a heading of the car in radians
+    :param quaternion: quaternion of the car as a list [q.x, q.y, q.z, q.w]
+                       where q is the quaternion
+    :return: heading of the car in radians (float)
+    """
+    # Create a Rotation object from the quaternion
+    rotation = Rotation.from_quat(quaternion)
+    # Convert the Rotation object to a matrix
+    rotation_matrix = rotation.as_matrix()
+    # calculate the angle around the z-axis (theta) from the matrix
+    # (see ../../doc/06_perception/00_coordinate_transformation.md)
+    theta = np.arctan2(rotation_matrix[1, 0], rotation_matrix[0, 0])
+
+    raw_heading = theta
+
+    # transform raw_heading so that:
+    # ---------------------------------------------------------------
+    # | 0 = x-axis | pi/2 = y-axis | pi = -x-axis | -pi/2 = -y-axis |
+    # ---------------------------------------------------------------
+    # The above transformation limits the heading to the range of -pi to pi
+    # It also rotates the heading by 90 degrees so that the heading is in
+    # the direction of the x-axis which the car starts in (heading = 0)
+
+    # heading is positive in counter clockwise rotations
+    heading = (raw_heading - (math.pi / 2)) % (2 * math.pi) - math.pi
+
+    return heading
+
+```