diff --git a/.gitignore b/.gitignore index 33b82e2d..bf3c7859 100644 --- a/.gitignore +++ b/.gitignore @@ -11,4 +11,3 @@ code/output # Byte-compiled / optimized / DLL files __pycache__/ - diff --git a/build/docker-compose.yml b/build/docker-compose.yml index a68e97c4..1545f945 100644 --- a/build/docker-compose.yml +++ b/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: diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index 975d0b0e..b1262c4b 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -29,10 +29,10 @@ - + diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index e5328799..1100405d 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -31,6 +31,7 @@ Heading Filter values: - "Kalman" (Default) - "None" + - "Old" (Buggy for demonstration purposes only) --> diff --git a/code/perception/src/Position_Publisher_Node.py b/code/perception/src/Position_Publisher_Node.py index 66e438f4..25a56f30 100755 --- a/code/perception/src/Position_Publisher_Node.py +++ b/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 diff --git a/code/perception/src/kalman_filter.py b/code/perception/src/kalman_filter.py index 67f9e36b..9ee3d242 100755 --- a/code/perception/src/kalman_filter.py +++ b/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]) diff --git a/doc/00_assets/perception/data_26_MAE_Boxed.png b/doc/00_assets/perception/data_26_MAE_Boxed.png new file mode 100644 index 00000000..c80b7a61 Binary files /dev/null and b/doc/00_assets/perception/data_26_MAE_Boxed.png differ diff --git a/doc/00_assets/perception/data_26_MSE_Boxed.png b/doc/00_assets/perception/data_26_MSE_Boxed.png new file mode 100644 index 00000000..f5c4a3c9 Binary files /dev/null and b/doc/00_assets/perception/data_26_MSE_Boxed.png differ diff --git a/doc/00_assets/perception/kalman_installation_guide.png b/doc/00_assets/perception/kalman_installation_guide.png new file mode 100644 index 00000000..f571273d Binary files /dev/null and b/doc/00_assets/perception/kalman_installation_guide.png differ diff --git a/doc/00_assets/perception/quat_to_angle.png b/doc/00_assets/perception/quat_to_angle.png new file mode 100644 index 00000000..298c632c Binary files /dev/null and b/doc/00_assets/perception/quat_to_angle.png differ diff --git a/doc/06_perception/00_coordinate_transformation.md b/doc/06_perception/00_coordinate_transformation.md index cafc83fd..3a54cbf5 100644 --- a/doc/06_perception/00_coordinate_transformation.md +++ b/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 -- [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) -## 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 + +``` diff --git a/doc/06_perception/08_kalman_filter.md b/doc/06_perception/08_kalman_filter.md index 7ba78703..d3a0b1a9 100644 --- a/doc/06_perception/08_kalman_filter.md +++ b/doc/06_perception/08_kalman_filter.md @@ -4,6 +4,8 @@ The Kalman Filter node is responsible for filtering the location and heading data, by using an IMU and GNSS sensor together with the carla speedometer. +As of now it is working with a 2D x-y-Transition model, which is why the current z-pos is calculated with a rolling average. + --- ## Author @@ -27,20 +29,26 @@ Robert Fischer - [1. Predict](#1-predict) - [2. Update](#2-update) - [3. Publish Data](#3-publish-data) + - [Also Important](#also-important) - [Inputs](#inputs) - [Outputs](#outputs) + - [Performance](#performance) --- ## Getting started -Right now the Node does not work correctly. It creates topics to publish to, but doesn't yet. -This will be fixed in [#106](https://github.com/una-auxme/paf23/issues/106) - Uncomment the kalman_filter.py node in the [perception.launch](../../code/perception/launch/perception.launch) to start the node. -You can also uncomment the rqt_plots that seem useful to you. -No extra installation needed. + +Also change the pos_filter and heading_filter parameter values of the Position_Publisher_Node in the [perception.launch](../../code/perception/launch/perception.launch) file, +to **"Kalman"**, depending on if you want to use the Filter for both the Position and the Heading. + +In the case of using the Filter for both, it should look like this: + +![Kalman Filter for both parameters](../../doc/00_assets/perception/kalman_installation_guide.png) + +No further installation needed. --- @@ -59,28 +67,67 @@ Stackoverflow and other useful sites: [3](https://stackoverflow.com/questions/66167733/getting-3d-position-coordinates-from-an-imu-sensor-on-python), [4](https://github.com/Janudis/Extended-Kalman-Filter-GPS_IMU) -This script implements a Kalman Filter. It is a recursive algorithm used to estimate the state of a system that can be modeled with linear equations. -This Kalman Filter uses the location provided by a GNSS sensor (by using the current_pos provided by the [Position Publisher Node](../../code/perception/src/Position_Publisher_Node.py)) +This script implements a Kalman Filter. + +It is a recursive algorithm used to estimate the state of a system that can be modeled with linear equations. + +This Kalman Filter uses the location provided by a GNSS sensor (by using the unfiltered_ provided by the [Position Publisher Node](../../code/perception/src/Position_Publisher_Node.py)) the orientation and angular velocity provided by the IMU sensor and the current speed in the headed direction by the Carla Speedometer. +As of now it is working with a 2D x-y-Transition model, which is why the current z-pos is calculated with a [rolling average](#also-important). + ```Python + The state vector X is defined as: [initial_x], [initial_y], + [v_x], + [v_y], [yaw], - [v_hy] in direction of heading hy, - [a_hy] in direction of v_hy (heading hy), - [omega_z], -The state transition matrix F is defined as: - A = I - I: Identity Matrix + [omega_z] + +The state transition matrix A is defined as: + ''' + # [x ... ] + # [y ... ] + # [v_x ... ] + # [x_y ... ] + # [yaw ... ] + # [omega_z ... ] + x = x + v_x * dt + y = y + v_y * dt + v_x = v_x + v_y = v_y + yaw = yaw + omega_z * dt + omega_z = omega_z + ''' + A = np.array([[1, 0, self.dt, 0, 0, 0], + [0, 1, 0, self.dt, 0, 0], + [0, 0, 1, 0, 0, self.dt], + [0, 0, 0, 1, 0, 0], + [0, 0, 0, 0, 1, 0], + [0, 0, 0, 0, 0, 1]]) + The measurement matrix H is defined as: - H = [1, 0, 0, 0, 0, 0], - [0, 1, 0, 0, 0, 0], - [0, 0, 1, 0, 0, 0], - [0, 0, 0, 0, 0, 1] + ''' + 1. GPS: x, y + 2. Velocity: v_x, v_y + 3. IMU: yaw, omega_z + -> 6 measurements for a state vector of 6 + ''' + self.H = np.array([[1, 0, 0, 0, 0, 0], # x + [0, 1, 0, 0, 0, 0], # y + [0, 0, 1, 0, 0, 0], # v_x + [0, 0, 0, 1, 0, 0], # v_y + [0, 0, 0, 0, 1, 0], # yaw + [0, 0, 0, 0, 0, 1]]) # omega_z + The process covariance matrix Q is defined as: - Q = np.diag([0.005, 0.005, 0.001, 0.0001]) + self.Q = np.diag([0.0001, 0.0001, 0.00001, 0.00001, 0.000001, 0.00001]) + +The measurement covariance matrix R is defined as: + self.R = np.diag([0.0005, 0.0005, 0, 0, 0, 0]) + ``` Then 3 Steps are run in the frequency of the `control_loop_rate`: @@ -88,48 +135,82 @@ Then 3 Steps are run in the frequency of the `control_loop_rate`: ### 1. Predict ```Python - # Update the old state and covariance matrix - self.x_old_est[:, :] = np.copy(self.x_est[:, :]) - self.P_old_est[:, :] = np.copy(self.P_est[:, :]) - # Predict the next state and covariance matrix - self.x_pred = self.A @ self.x_est[:] # + B @ v[:, k-1] + u - self.P_pred = self.A @ self.P_est[:, :] @ self.A.T + self.Q + # Predict the next state and covariance matrix, pretending the last + # velocity state estimate stayed constant + self.x_pred = self.A @ self.x_est + self.P_pred = self.A @ self.P_est @ self.A.T + self.Q + ``` ### 2. Update ```Python - z = np.concatenate((self.z_gps[:], self.z_imu[:])) # Measurementvector - y = z - self.H @ self.x_pred # Measurement residual - S = self.H @ self.P_pred @ self.H.T + self.R # Residual covariance - self.K[:, :] = self.P_pred @ self.H.T @ np.linalg.inv(S) # Kalman gain - self.x_est[:] = self.x_pred + self.K[:, :] @ y # State estimate - # State covariance estimate - self.P_est[:, :] = (np.eye(6) - self.K[:, :] @ self.H) @ self.P_pred + + # Measurementvector z + z = np.concatenate((self.z_gps, self.z_v, self.z_imu)) + # Measurement residual y + y = z - self.H @ self.x_pred + # Residual covariance S + S = self.H @ self.P_pred @ self.H.T + self.R + # Kalman gain K + self.K = self.P_pred @ self.H.T @ np.linalg.inv(S) + # State estimate x_est + self.x_est = self.x_pred + self.K @ y + # State covariance estimate P_est + # (Joseph form of the covariance update equation) + self.P_est = (np.eye(6) - self.K @ self.H) @ self.P_pred + ``` ### 3. Publish Data ```Python + # Publish the kalman-data: self.publish_kalman_heading() self.publish_kalman_location() + +``` + +### Also Important + +The way that the xyz Position is made, only x and y are measured for the kalman filter, while the z component is filtered by a rolling average. + +```Python + + GPS_RUNNING_AVG_ARGS: int = 10 + + self.avg_z = np.zeros((GPS_RUNNING_AVG_ARGS, 1)) + . + . + . + # update GPS Measurements: + self.z_gps[0, 0] = current_pos.pose.position.x + self.z_gps[1, 0] = current_pos.pose.position.y + + z = current_pos.pose.position.z + + self.avg_z = np.roll(self.avg_z, -1, axis=0) + self.avg_z[-1] = np.matrix([z]) + avg_z = np.mean(self.avg_z, axis=0) + + self.latitude = avg_z + ``` -As stated before, the script does not publish viable data yet, which has to be fixed. -This means the predict and update steps might not be correct, because it wasn't possible to test it yet. +The Kalman Location as well as the Kalman Heading are then subscribed to by the PositionPublisher and republished as **current_heading** and **current_pos** ### Inputs This node subscribes to the following needed topics: - IMU: - - `/carla/{role_name}/Ideal_IMU` ([IMU](https://docs.ros.org/en/api/sensor_msgs/html/msg/Imu.html)) + - `/carla/{role_name}/IMU` ([IMU](https://docs.ros.org/en/api/sensor_msgs/html/msg/Imu.html)) - GPS: - - `/carla/{role_name}/Ideal_GPS` ([NavSatFix](http://docs.ros.org/en/melodic/api/std_msgs/html/msg/String.html)) -- current agent position: - - `/paf/{role_name}/current_pos` ([PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)) + - `/carla/{role_name}/GPS` ([NavSatFix](http://docs.ros.org/en/melodic/api/std_msgs/html/msg/String.html)) +- unfiltered agent position: + - `/paf/" + self.role_name + "/unfiltered_pos` ([PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)) - Carla Speed: - `/carla/" + self.role_name + "/Speed` CarlaSpeedometer @@ -138,6 +219,28 @@ This node subscribes to the following needed topics: This node publishes the following topics: - Kalman Heading: - - `/paf/{role_name}/kalman_heading` ([PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)) + - `/paf/{role_name}/kalman_heading` ([Float32](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float32.html)) - Kalman Position: - `/paf/{self.role_name}/kalman_pos` ([PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)) + +## Performance + +In the following graphs you will see the MSE/ MAE Boxed Graph of Location Error with respect to ideal Location. + +Lower values in the data mean smaller error values. + +Smaller boxes mean the data is closer together and less spread. + +The Kalman Filter was tuned to create the smallest MSE possible, which gives more weight to larger errors which we want to minimise. + +The MAE on the other hand shows a 1:1 representation in terms of distance from the ideal to the predicted location. +![MSE Boxed Graph of Location Error with respect to ideal Location](../../doc/00_assets/perception/data_26_MSE_Boxed.png) + +![MAE Boxed Graph of Location Error with respect to ideal Location](../../doc/00_assets/perception/data_26_MAE_Boxed.png) + +As you see this data you might think the unfiltered data seems to be just as good if not even better than the previous rolling average filter (RAF). + +This is not the case, since the RAF has a way smaller spread of datapoints, making the location data way smoother (Though with a large error) than without a filter. + +The Kalman Filter on the other hand makes it possible for the data to be way below the 1 m error mark (with 0.48 m as its error median) +and contain the data within a reasonable range. diff --git a/doc/06_perception/09_position_publisher_node.md b/doc/06_perception/09_position_publisher_node.md new file mode 100644 index 00000000..bc2fe25f --- /dev/null +++ b/doc/06_perception/09_position_publisher_node.md @@ -0,0 +1,116 @@ +# Position Publisher Node + +**Summary:** This node publishes the `current_pos` (Location of the car) and `current_heading` (Orientation of the car around the Z- axis) for every Node that needs to work with that. It also publishes all relevant topics for the Filter nodes (such as Kalman). + +--- + +## Author + +Robert Fischer + +## Date + +14.01.2024 + +## Prerequisite + +* [Position Publisher Node](#position-publisher-node) + * [Author](#author) + * [Date](#date) + * [Prerequisite](#prerequisite) + * [Usage](#usage) + * [Heading Functions](#heading-functions) + * [Position Functions](#position-functions) + * [Inputs](#inputs) + * [Outputs](#outputs) + +## Usage + +Change the pos_filter and heading_filter parameter values of the Position_Publisher_Node in the [perception.launch](../../code/perception/launch/perception.launch) file, +to **"Kalman"**, depending on if you want to use the Kalman Filter for both the Position and the Heading. + +You will also have to uncomment the kalman_filter.py node or any other filter in the [perception.launch](../../code/perception/launch/perception.launch) to start the filter node. +The only filter that is already implementet by the Position Publisher Node is the RunningAverage Filter, which can be turned on the same way. + +You can use filters for the heading and for the location independently using the following parameter values: + +**Position Filter values:** + - "Kalman" (Default) + - "RunningAvg" + - "None" +**Heading Filter values:** + - "Kalman" (Default) + - "None" + - "Old" (Buggy for demonstration purposes only) + +In the case of using the Filter for both, it should look like this: + +![Kalman Filter for both parameters](../../doc/00_assets/perception/kalman_installation_guide.png) + +_If you want to create a new Filter in the future, I suggest keeping this template intact._ 😊 + +No further installation needed. + +### Heading Functions + +**`publish_unfiltered_heading`** + +This method is called when new IMU data is received. +It publishes unfiltered location data as `PoseStamped`, so other filters can use it. + +If `none` is selected for the Filter, it publishes the data as the `current_pos` instead. (not recommended) + +**`publish_current_heading`** + +This method is called when new heading data is received. It handles all necessary updates and publishes the heading as a double value, +indicating the cars rotation around the z-axis in rad. +For more info about how the heading is calculated see [here](./00_coordinate_transformation.md). + +### Position Functions + +**`publish_running_avg_pos`** + +This method is called when new GNSS data is received. The function calculates the average position and then publishes it as the current position. + +This is only used when the `running_avg` Filter is selected. + +**`publish_kalman_pos_as_current_pos`** + +This method is called when new kalman filter data is received. The function publishes the kalman position as the current position. + +This is only used when the `Kalman` Filter is selected + +**`publish_unfiltered_gps`** + +This method is called when new GNSS data is received. + +It publishes unfiltered location data as `PoseStamped`, so other filters can use it. + +If `none` is selected for the Filter, it publishes the data as the `current_pos` instead. (not recommended) + +**`get_geoRef`** + +Reads the reference values for lat and lon from the carla OpenDriveMap. +Otherwise we could not calculate the global Coordinate System + +### Inputs + +This node subscribes to the following topics: + +* IMU: + * `/carla/{role_name}/IMU` ([IMU](https://docs.ros.org/en/api/sensor_msgs/html/msg/Imu.html)) +* GPS: + * `/carla/{role_name}/GPS` ([NavSatFix](http://docs.ros.org/en/melodic/api/std_msgs/html/msg/String.html)) +* Kalman position: + * `/paf/" + self.role_name + "/kalman_pos` ([PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)) +* Kalman Heading: + * `/paf/" + self.role_name + "/kalman_heading` ([Float32](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float32.html)) + +### Outputs + +This node publishes the following topics: + +* Current Heading: + * `/paf/{role_name}/current_heading` ([Float32](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float32.html)) +* Current Position: + * `/paf/{self.role_name}/current_pos` ([PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)) diff --git a/doc/06_perception/Readme.md b/doc/06_perception/Readme.md index f2942fb3..a436f3cc 100644 --- a/doc/06_perception/Readme.md +++ b/doc/06_perception/Readme.md @@ -19,3 +19,5 @@ This folder contains further documentation of the perception components. ## 7. [sensor_filter_debug.py](./07_sensor_filter_debug.md) ## 8. [Kalman Filter](./08_kalman_filter.md) + +## 9. [Position Publisher Node](./09_position_publisher_node.md)