2.2.4.7. slam module

SLAM ROS Module to track and estimate vehicle pose and track defined by landmarks in form of cones.

Vehicle pose is defined as $\overrightarrow{x_R}=\left(\begin{array}{cccc}x& y& v_x& \psi \end{array}\right)$.

An UKF is used to track and estimate the vehicle pose and the track. This UKF is implemented in ukf.UKF.

Since measurements containing the position of cones are delayed by upto $400$ ms, using only one filter to estimate the vehicle pose and the track at the same time would mean that the latest vehicle pose would also be delayed by upto $400$ ms and more.

Thus SLAM uses two independent filters, one saved slam.SLAM.realtime_filter for estimating the vehicle pose without using landmark measurements in realtime. The other one saved in slam.SLAM.main_filter estimates the vehicle pose and the landmarks in one state vector, defined as $\overrightarrow{x}=\left(\begin{array}{ccccccccc}x& y& v_x& \psi & x_{l,0}& y_{l,0}& \dots & x_{l,n}& y_{l,n}\end{array}\right)$ with n equal the number of tracked landmarks.

To enable this, the measurements of the different sensors cannot be used just in time and rather have to be buffered until they are used by the main filter. Therefore received measurements by wheelspeed sensors, IMU, GPS, local mapping and steering wheel angle sensors are buffered in slam.SLAM.steering_u_array, slam.SLAM.accel_u_array, slam.SLAM.gyro_u_array and the respective sensors.sensor.Sensor.ros_z_array. The respective timestamp for the buffered measurement is saved in the respective t_array. Also, if the Sensor publishes the uncertainty aswell, the uncertainty is buffered in the respective sensors.sensor.Sensor.ros_R_array.

When executing a filter, the respective buffered measurements and control inputs for the timestamp are retrieved and set to the filter or sensors (slam.SLAM.set_measurements() and slam.SLAM.set_u_from_buffer()).

In normal operation, all incoming measurements via ROS are buffered in mentioned buffered, except the local mapping measurements. They get discarded until the realtime filter has been executed atleast once. The module waits until all control inputs are received atleast once. Afterwards the realtime filter is executed reguarly with a rate defined in ukf.UKF.predict_tolerance. The states and their uncertainty after each execution step are buffered in slam.SLAM.x_array and slam.SLAM.P_array until the main filter is initialized.

As soon as the first local mapping measurement is received, the thread for the main filter is started and the main filter is intialized with the states and their uncertainty buffered for the latest timestamp before the local mapping measurement. Afterwards the main filter is always executed when new local mapping measurements are received. The main filter is executed with a maximum $dt$ equal ukf.UKF.predict_tolerance.

Preloading

In case preloading is activated, the map to be preloaded is loaded into slam.SLAM.preload_filter and slam.SLAM.preloaded_centerpoints. The logic where to maps are saved and the logic to decide which one to choose is decribed in slam.SLAM.try_preload_map(). If the alignment of the preloaded map (slam.SLAM.preloaded_map_alignment_active) is deactived, the map from the preloaded filter is subsequently loaded into the main filter.

Otherwise there are two ways to align the preloaded map, chosen via slam.SLAM.use_gps_instead_of_tracked_map. The first approach uses the first used GPS Measurement (by calling slam.SLAM.try_aligning_preloaded_map_via_gps() during execution of realtime filter) and the GPS Coordinates for the coordinate origin of the preloaded map, to align both. This assumes that the preloaded map contains the gps coordinates for the coordinate origin, which are saved under slam.SLAM.preloaded_gps_measurements after preloading the map.

The second approach will execute the realtime and main filter as in normal operation. Except, when enough cones have been perceived and tracked (slam.SLAM.minimum_preload_landmarks and slam.SLAM.alignment_perceived_n_threshold, see slam.SLAM.try_aligning_preloaded_map_via_main_filter()), a data association between the tracked map and preloaded map is calculated. Subsequently the preloaded map is rotated and translated to align both maps according to the data association. Afterwards the preloaded map is loaded into the main filter.

After loading the preloaded map into the main filter, slam.SLAM.preloading_keep_updating decides whether the landmarks should still be updated and thus whether the main filter and the perception node should be kept active.

Also, if the Autonomous System drives Skidpad or Acceleration, preloaded centerpoints are published to ROS. First, when the map has been preloaded into the preload filter and second, in case the preloaded map has been aligned and loaded into the main filter. This assumes that the preloaded map contained centerpoints. This is necessary if the Autonomous System drives Acceleration or Skidpad, since Filtering will shutdown in these cases automatically.

When the Autonomous System shutdowns (someone kills the simulation or the mission machine kills it), the tracked map will be saved on shutdown to the according directory with an auto-incrementing number so it can be preloaded for the next, possible run.

Conclusion

We have two seperate and independent UKF instances, one for tracking the vehicle pose in realtime (realtime filter) and one to track the map as soon as the local mapping measurements are available (main filter).

The map for the second filter can be preloaded from existings maps. These preloaded maps can be aligned either via GPS or via aligning a tracked map with the preloaded map.

The second filter can be deactivated as soon as a preloaded map has been loaded into the main filter, or the Autonomous System perceived all parts of the map (Autocross: one lap driven, Skidpad: both circles driven).

Outlook

Since we use two independent filter their vehicle poses can diverge a lot over the time, especially when no GPS measurements are used. When this happens, the main filter probably will track the more precise vehicle pose since it can use the landmark measurements to improve the estimated vehicle pose.

To solve this problem there exist a possible approach which already was implemented once, but the support was dropped since the advantage was marginally when using GPS Measurements and the filter where not perfomant enough back then:

The idea is to fork a new realtime filter from the main filter from time to time. This is not possible directly, since this realtime filter would be not upto date anymore. Thus the a third filter was introduces: The catchup filter. The catchup filter was introduced as soon as the last timestamp of the realtime filter with local mapping measurements was older than x seconds and since then there were atleast one new timestamp with local mapping measurements. Then, the catchup filter was forked from the latest timestamp of the main filter with local mapping measurements and was executed for with ukf.UKF.predict_tolerance incrementing timestamps until it reached the realtime filter. Then the catchup filter was used as new realtime filter, until a new catchup filter was forked from the main filter and reached the realtime filter. And so on. This might be an idea for the future. Please call it Ketchup filter in the next implementation. Just for the heck of it. Thanks.

class slam.AutonomousMissionEnum(value)

Bases: Enum

Enum to describe which Autonomous Mission is currently select.

Table 2.3 Mission IDs

Mission ID	Mission Name
0	Manual Driving
1	Acceleration
2	Skidpad
3	Trackdrive
4	Braketest / EBS Test
5	Inspection
6	Autocross

ACCELERATION = 1

AUTOCROSS = 6

BRAKETEST = 4

INSPECTION = 5

MANUAL_DRIVING = 0

SKIDPAD = 2

TRACKDRIVE = 3

class slam.SLAM

Bases: object

Test.

mission

Autonomous Mission which is currently driven.

Type:

AutonomousMissionEnum

realtime_filter

UKF to estimate and track vehicle pose in realtime with as little as possible delay.

Type:

UKF

main_filter

UKF to estimate and track vehicle pose and track at the same time but delayed.

Type:

Optional[UKF]

main_filter_active

Indicates whether main filter is still activated.

Type:

bool

disable_perception_when_inactive

Indicates whether to disable perception when main filter is not updating landmarks anyway.

Type:

bool

send_debug_informations

Indicates whether debug informations should be send to ROS, e.g. prior vehicle poses, predicted measurements and map alignment information.

Type:

bool

imu_source

Indicates whether to use GPS IMU or Aceinna IMU as source for control input $u$.

Type:

str

preload_filter

UKF to act as container for preloaded map for buffering until preloaded map is aligned and loaded into main filter.

Type:

UKF

preloaded_gps_measurements

GPS Measurements preloaded which would be measured at map origin, must not be present.

Type:

dict

preloaded_centerpoints

Centerpoints of preloaded map, must not be present.

Type:

dict

preload_aligned

Indicates whether preloaded map has already been aligned.

Type:

bool

use_landmark_for_alignment

Array indicating whether the respective landmark should be used for registration during alignment of preloaded map and estimated & tracked landmarks, shape: (n ) with n equal number of prelaoded landmarks.

Type:

Optional[npt.NDArray]

preloading_active

Indicates whether preloading is activated.

Type:

bool

use_gps_instead_of_tracked_map

Indicates whether preloaded map and estimated & tracked map should be aligned via GPS instead of registration.

Type:

bool

minimum_preload_landmarks

Number of landmarks that must be estimated and tracked before preloaded map and estimated & tracked map are aligned.

Type:

int

alignment_perceived_n_threshold

Number of times a landmark has to been perceived before it is usable for map alignment.

Type:

int

preloading_added_width

Width [m] to be added around polygon containing tracked & estimated landmarks for computing observable preloaded landmarks.

Type:

float

preloading_added_height

Height [m] to be added around polygon containing tracked & estimated landmarks for computing observable preloaded landmarks.

Type:

float

preloaded_map_alignment_active

Indicates whether preloaded map should be aligned rather than directly loaded into main filter.

Type:

bool

preloading_keep_updating

Indicates whether preloaded landmarks still should be updated.

Type:

bool

align_event

Event to indicate that map alignment can be execute since enough landmarks are estimated & tracked.

Type:

threading.Event

align_thread

Thread to execute the alignment of the estimated & tracked map and the preloaded map.

Type:

threading.Thread

max_age_of_local_mapping_measurements

Maximum age of local mapping measurements [s] to be used during update of main filter, otherwise get discarded.

Type:

float

x_array

Array containing buffered states $\overrightarrow{x}$ of realtime filter, shape: (x, ) with x equal number of buffered states.

Type:

List[npt.NDArray]

P_array

Array containing buffered covariance matrices $\overrightarrow{P}$ of realtime filter, shape: (x, ) with x equal number of buffered states.

Type:

List[npt.NDArray]

t_array

Array containing the timestamps states $\overrightarrow{x}$ and covariance matrices $\overrightarrow{P}$ of realtime filter have been buffered for, shape: (x, ) with x equal number of buffered states.

Type:

List[float]

wheelspeed_rr

Wheelspeed sensor representing the rear right wheelspeed sensor of the car.

Type:

CUREDiffWheelspeed

wheelspeed_rl

Wheelspeed sensor representing the rear left wheelspeed sensor of the car.

Type:

CUREDiffWheelspeed

wheelspeed_fr

Wheelspeed sensor representing the front right wheelspeed sensor of the car.

Type:

CUREDiffWheelspeed

wheelspeed_fl

Wheelspeed sensor representing the front left wheelspeed sensor of the car.

Type:

CUREDiffWheelspeed

gps_heading

Sensor representing the heading measurements of the Novatel GPS.

Type:

NovatelGPSHeading

gps_position

Sensor representing the position and speed measurements of the Novatel GPS.

Type:

NovatelGPSHeading

local_mapping

Sensor representing the measurements of the local mapping module.

Type:

LocalMapping

local_mapping_subscriber

Subscriber for measurements from local mapping module.

Type:

rospy.Subscriber

camera_offset

Offset [m] between rear axle and camera, retrieved from ROS transform tree.

Type:

Optional[float]

steering_u_array

Array containing buffered measured steering wheel angles [deg] for control input $\overrightarrow{u}$, shape: (s, ) with s equal number of steering wheel angles.

Type:

List[float]

steering_t_array

Array containing timestamps for which measured steering wheel angles have been buffered, shape: (s, ) with s equal number of steering wheel angles.

Type:

List[float]

accel_u_array

Array containing buffered measured longitudinal accelerations [m/s] for control input $\overrightarrow{u}$, shape: (a, ) with a equal number of longitudinal accelerations.

Type:

List[float]

accel_t_array

Array containing timestamps for which measured longitudinal accelerations have been buffered, shape: (a, ) with a equal number of longitudinal accelerations.

Type:

List[float]

gyro_u_array

Array containing buffered measured yaw rates [deg/s] for control input $\overrightarrow{u}$, shape: (y, ) with y equal number of buffered yaw rates.

Type:

List[float]

gyro_t_array

Array containing timestamps for which measured yaw rates have been buffered, shape: (y, ) with y equal number of buffered yaw rates.

Type:

List[float]

rotation_matrix

Rotation matrix to compensate for possible inclination of Aceinna IMU, retrieved from ROS transform tree.

Type:

npt.NDArray

steering_u_lock

Mutual exclusion lock so that steering wheel measurements are not added, retrieved or deleted at the same time.

Type:

threading.Lock

accel_u_lock

Mutual exclusion lock so that acceleration measurements are not added, retrieved or deleted at the same time.

Type:

threading.Lock

gyro_u_lock

Mutual exclusion lock so that gyro measurements are not added, retrieved or deleted at the same time.

Type:

threading.Lock

realtime_lock

Mutual exclusion lock so that realtime filter is not used from mutliple threads at the same time.

Type:

threading.Lock

main_lock

Mutual exclusion lock so that main filter is not used from mutliple threads at the same time.

Type:

threading.Lock

main_thread

Thread for executing the main filter.

Type:

threading.Thread

steering_model

Steering model defined by steering characteristics.

Type:

dict

max_steering_wheel_angle

Boundaries of steering wheel angle [deg].

Type:

float

steering_wheel_angle_offset

Offset of steering wheel angle [deg] at which vehicle should drive straight.

Type:

float

lap_count

Number of laps driven received by finish line detection.

Type:

int

transform_listener

Listener for ROS transform tree. Used to retrieve transform from rear axle to camera and Aceinna IMU.

Type:

tf2_ros.TransformListener

tf_buffer

Buffer for slam.SLAM.transform_listener.

Type:

tf2_ros.Buffer

Initialize SLAM ros node.

align_preloaded_map()

Align preloaded map via perceived and tracked landmarks.

First, wait for the event to be set that enough landmarks have been tracked that preloaded map can be aligned.

1. Get the data from the main filter

2. Compute data association between preloaded map and tracked landmarks via JCBB

3. Compute best transform between preloaded map and associated tracked landmarks

4. Align centerpoints if they have been preloaded previously

5. Load preloaded map into main filter

6. Send debug informations of map alignment to ROS, if configured

buffer_x_and_P(current_timestamp: float)

Buffer current state vector $x$ and covariance matrix $P$ of realtime filter for current timestamp.

Buffer current state vector $x$ to slam.SLAM.x_array and covariance matrix $P$ to to slam.SLAM.P_array.

Parameters:

current_timestamp (float) – Timestmap for which the current state vector $x$ and covariance matrix $P$ should be buffered.

correct_offsets()

Correct camera and lidar offsets of local mapping sensor by extracting values from transformation tree.

delete_measurements(filter: UKF)

Delete all measurements $\overrightarrow{z}$ for all sensors by setting the array values to np.nan.

disable_perception()

Disabling perception by sending a ROS message to inference node.

enable_perception()

Enable perception again by sending a ROS message to inference node after disabling it.

execute_filter_for_timestamp(filter: UKF, timestamp: float, use_local_mapping: bool = False)

Execute predict and update for a filter.

1. Delete measurements for all sensors for the given filter used in the last timestamp.

2. Set measurements from the buffered ROS messages for the given timestamp and filter.

3. Prepare the update by preparing every sensor and calcuting which states have to be predicted and updated.

4. Predict.

5. Update.

6. Postprocess the update by post processing every sensor.

execute_main_filter()

Execute main filter.

Repeats the following as long as main filter is activated and until ROS is shutdown:

1. If main filter is not set yet but local mapping measurements available

   1. Get vehicle pose $\overrightarrow{x}$ and its uncertainty $\overrightarrow{P}$ from the buffers slam.SLAM.x_array and slam.SLAM.P_array

   2. Copy realtime filter to main filter for respective timestamp with respective $\overrightarrow{x}$ and $\overrightarrow{P}$

2. Until current timestamp of main filter is less than ukf.UKF.predict_tolerance away from oldest unused local mapping measurement

   1. Execute main filter for ukf.UKF.t + ukf.UKF.predict_tolerance

   2. Send vehicle pose $\overrightarrow{x_R}$ to ROS

   3. Send prior vehicle pose $\overrightarrow{x_{R,prior}}$ to ROS

   4. Send predicted measurements and more debug informations to ROS, if configured.

3. Execute main filter for timestamp of oldest unused local mapping measurement

4. Send vehicle pose $\overrightarrow{x_R}$ to ROS

5. Send prior vehicle pose $\overrightarrow{x_{R,prior}}$ to ROS

6. Send predicted measurements and more debug informations to ROS, if configured.

7. Send tracked landmarks to ROS if main filter is still active.

execute_real_time_filter()

Execute realtime filter.

Repeats the following until ROS is shutdown:

1. Execute realtime filter for current timestamp

2. Send vehicle pose $\overrightarrow{x_R}$ to ROS

3. Send predicted measurements and more debug informations to ROS, if configured.

4. Try to align preloaded map via gps, in case its configured and GPS measurements already received.

5. Send prior vehicle pose $\overrightarrow{x_{R,prior}}$ to ROS, if configured

6. Buffer estimated states $\overrightarrow{x}$ and their uncertainty $\overrightarrow{P}$ if main filter not existing yet

7. Pop previously buffered states and their uncertainty if main filter is deactivated

static get_u_from_input_buffer(t_array: List[float], u_array: List[float], timestamp: float) → float

Get latest control input $u$ for given control input buffer which lies before given timestamp.

Parameters:

• t_array (List[float]) – Buffer for timestamps for respective buffered control input $u$, shape: (n, ) with n equal number of buffered timesteps.

• u_array (List[float]) – Buffer for control input $u$, shape: (n, ) with n equal number of buffered timesteps.

• timestamp (float) – Timestamp for which the latest control input $u$ should be get.

Returns:

Latest control input $u$ for given control input buffer which lies before given timestamp.

Return type:

float

get_x_and_P_from_buffer(local_mapping_timestamp: float) → Tuple[ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]], float]

Get latest buffered state vector $x$ and covariance matrix $P$ which are before the given timestamp.

Parameters:

local_mapping_timestamp (float) – Timestamp for which the latest state vector and covariance matrix should be returned.

Returns:

State vector $x$, covariance matrix $P$ and timestamp of both.

Return type:

Tuple[npt.NDArray, npt.NDArray, float]

Raises:

RuntimeError – No timestamp buffered suitable for given timestamp.

gps_heading_callback(gps_heading: HEADING2)

Buffer gps heading measurement incusive its uncertainty.

Buffers new measurements to the sensors.novatel.NovatelGPSHeading.ros_z and sensors.novatel.NovatelGPSHeading.ros_R attribute of the respective sensor (slam.SLAM.gps_heading).

Before buffering, the sensors.novatel.NovatelGPSHeading.offset is added to the measurements, to compensate that the GPS antennas might be mounted not inline with the vehicle x axis.

Parameters:

gps_heading (HEADING2) – ROS message containing gps heading measurement and its uncertainty.

gps_imu_callback(gps_imu: Imu)

Buffer acceleration measurements from GPS IMU.

Buffers new measurements to the slam.SLAM.accel_u_array and slam.SLAM.gyro_u_array attribute. Measurements are not rotated since the GPS does this already internally.

Parameters:

gps_imu (Imu) – ROS message containing GPS IMU measurements.

gps_position_callback(gps_position: GPSFix)

Buffer gps position and speed measurements inclusive their uncertainty from the GPS.

Buffers new measurements to the sensors.novatel.NovatelGPSPosition.ros_z and sensors.novatel.NovatelGPSPosition.ros_R attribute of the respective sensor (slam.SLAM.gps_position).

Parameters:

gps_position (GPSFix) – ROS message containing gps position measurements and their uncertainty.

imu_accel_callback(imu_accel: ImuAcceleration)

Buffer acceleration measurements from Aceinna IMU.

Buffers new measurements to the slam.SLAM.accel_u_array attribute. Only buffers when slam.SLAM.rotation_matrix is already looked up. Before buffering, measurements are rotated by slam.SLAM.rotation_matrix.

Parameters:

imu_accel (ImuAcceleration) – ROS message containing Aceinna IMU acceleleration measurement, (as_can_msgs.msg.ImuAcceleration).

imu_gyro_callback(imu_gyro: ImuGyro)

Buffer gyro measurements from Aceinna IMU.

Buffers new measurements to the slam.SLAM.gyro_u_array attribute. Only buffers when slam.SLAM.rotation_matrix is already looked up. Before buffering, measurements are rotated by slam.SLAM.rotation_matrix.

Parameters:

imu_gyro (ImuGyro) – ROS message containing Aceinna IMU gyro measurement, (as_can_msgs.msg.ImuGyro).

lap_count_callback(lap_count: LapCount)

Decide which filters to execute and whether to use perception based on lap count from finish line detection.

Strategy decision is based on the current mission, lap count and whether the filter is even active.

Trackdrive and Autocross

When the first lap is driven and the main filter is active the following is done:

1. Disable perception,

2. Change LandmarkUpdateStrategy of main filter to not updating anymore.

3. Deactivate main filter

4. Assume track to be closed.

5. Cleanup map, see ukf.UKF.cleanup_map()

6. Publish cleaned map

Skidpad

When the first lap is driven (aka the right circle is driven the first time) and the main filter is active the following things are done:

1. Disable perception,

2. Change LandmarkUpdateStrategy of main filter to not updating anymore.

3. Cleanup map, see ukf.UKF.cleanup_map()

4. Publish cleaned map

When the second lap is driven (aka the left circle has now to be driven the first time) and the main filter is active, the following things are done:

1. Enable perception,

2. Change LandmarkUpdateStrategy of main filter to update and initialize Landmarks but not vehicle pose anymore.

When the third lap is driven (aka the left circle is driven the first time) and the main filter is active, the following things are done:

1. Disable perception,

2. Change LandmarkUpdateStrategy of main filter to not updating anymore.

3. Deactivate main filter

4. Cleanup map, see ukf.UKF.cleanup_map()

5. Publish cleaned map

Parameters:

lap_count (LapCount) – Lap count counted by finish line detection and received via ROS, (as_can_msgs.msg.LapCount).

load_preloaded_filter_into_main_filter(landmarks: ndarray[Any, dtype[ScalarType]])

Load preloading filter into main filter.

Afterwards send preloaded landmarks and publish preloaded centerpoints if necessary. If configured, disable perception and main filter after loading landmarks into main filter. Otherwise stop filter from initializing any new landmarks when seeing new ones.

Parameters:

landmarks (npt.NDArray) – Landmarks to preload, shape: (n, 2) with n equals number of landmarks.

local_mapping_callback(cone_list: ConeList)

Buffer positions of perceived cones mapped by the local mapping module.

Cones are discarded if:

1. Main filter is deactived,

2. Cones are empty

3. Cones are older than slam.SLAM.max_age_of_local_mapping_measurements or

4. Main filter is not initialized yet and measurement lies before first realtime filter execution.

Buffers new measurements to the sensors.landmarks.LocalMapping.ros_z of the respective sensor (slam.SLAM.local_mapping).

Thread for executing the main filter is started when main filter is not initialized yet and thread not started yet.

Parameters:

cone_list (ConeList) – Ros message containing local mapping measurements, (utilities.msg.ConeList).

lookup_rotation_matrix()

Lookup rotation matrix for Aceinna IMU in transformation tree.

Saves rotation matrix in slam.SLAM.rotation_matrix. Unregister slam.SLAM.transform_listener when slam.SLAM.camera_offset is also already looked up.

on_shutdown()

Save tracked map on shutdown.

pop_from_buffer(local_mapping_timestamp: float)

Pop all buffered messages from buffers for control inputs and measurements preceding specified timestamp.

Parameters:

local_mapping_timestamp (float) – Timestamp until which all buffered messages should be popped.

pop_u_from_buffer(local_mapping_timestamp: float)

Delete all buffered control inputs $\overrightarrow{u}$ before given timestamp except each latest.

Parameters:

local_mapping_timestamp (float) – Timestamps before all buffered control inputs $\overrightarrow{u}$ should be deleted.

publish_centerpoints()

Publish preloaded and (if configured and applicable) aligned centerpoints to ROS.

This is especially important for skidpad, since it is very difficult to plan a centerline including a crossing. It is way easiert to preload this track and its centerline.

publish_centerpoints_if_necessary()

Publish preloaded and (if configured and applicable) aligned centerpoints to ROS if necessary.

It is necessary if we drive Skidpad, Acceleration or Braketest (EBS Test). Fatal if for those mission no preloaded centerpoints exists although a track was preloaded.

run()

Run SLAM.

First waits for every control input to be set, then executes the filter according to the defined dt / rate. Also correct camera/LiDAR offset according to transformation tree.

save_tracked_map()

Save tracked map to json file.

Tracked maps are saved under as_ros/src/slam/plots/maps/<test_day>/<track_layout>/map_<number>.json. Number is auto incrementing for the giben test day and track layout.

send_global_landmarks()

Publish tracked landmarks, flag whether track is closed and cone count to ROS.

send_main_prior_vehicle_pose()

Publish prior vehicle pose and its uncertainty of the main filter to ROS.

Publish prior vehicle pose $\overrightarrow{x_{R,prior}}$ and its uncertainty $\overrightarrow{P_{R,prior}}$ of the main filter to ROS.

send_main_vehicle_pose()

Publish vehicle pose $\overrightarrow{x_R}$ and its uncertainty $\overrightarrow{P_R}$ of the main filter to ROS.

send_predicted_measurements(filter: UKF, publisher: Publisher)

Publish predicted measurements and more to ROS if configured by slam.SLAM.send_debug_informations.

Among others statistics about data association.

send_prior_vehicle_pose(filter: UKF, publisher: Publisher)

Publish prior vehicle pose and its uncertainty of the respective filter to ROS.

Publish prior vehicle pose $\overrightarrow{x_{R,prior}}$ and its uncertainty $\overrightarrow{P_{R,prior}}$ of the respective filter to ROS.

Parameters:

• filter (UKF) – Filter of which prior vehicle pose should be published.

• publisher (rospy.Publisher) – Publisher for the prior vehicle pose of respective filter.

send_realtime_prior_vehicle_pose()

Publish prior vehicle pose and its uncertainty of the realtime filter to ROS.

Publish prior vehicle pose $\overrightarrow{x_{R,prior}}$ and its uncertainty $\overrightarrow{P_{R,prior}}$ of the realtime filter to ROS.

send_realtime_vehicle_pose()

Publish vehicle pose $\overrightarrow{x_R}$ and its uncertainty $\overrightarrow{P_R}$ of the realtime filter to ROS.

send_vehicle_pose(filter: UKF, publisher: Publisher)

Publish vehicle pose $\overrightarrow{x_R}$ and its uncertainty $\overrightarrow{P_R}$ of the respective filter to ROS.

Parameters:

• filter (UKF) – Filter of which vehicle pose should be published.

• publisher (rospy.Publisher) – Publisher for the vehicle pose of respective filter.

set_measurements(filter: UKF, timestamp: float, use_local_mapping: bool = False)

Set all measurements $\overrightarrow{z}$ to all sensors of respective filter for given timestamp.

Only measurements between given timestamp and ukf.UKF.predict_tolerance before are used. Measurements $\overrightarrow{z}$ from sensors.sensor.Sensor.ros_z_array and its uncertainity $\overrightarrow{R}$ from sensors.sensor.Sensor.ros_R_array are set to sensors.sensor.Sensor.ros_z and sensors.sensor.Sensor.ros_R.

Parameters:

• filter (UKF) – Filter of which the measurements $vecz$ should be set.

• timestamp (float) – Timestamp for which the latest measurements $vecz$ since last execution step should be set.

• use_local_mapping (bool, optional) – Whether measurements from local mapping should be set aswell, by default False

set_u_from_buffer(filter: UKF, timestamp: float)

Set all control inputs $\overrightarrow{u}$ to ukf.UKF.u of respective filter for given timestamp.

Parameters:

• filter (UKF) – Filter of which the control inputs $\overrightarrow{u}$ should be set.

• timestamp (float) – Timestamp for which the latest control inputs $\overrightarrow{u}$ should be set.

steering_wheel_angle_callback(steering_wheel_angle: SteeringWheelAngle)

Buffer steering wheel angle measurements.

Buffers new measurements to the slam.SLAM.steering_u_array attribute.

Parameters:

steering_wheel_angle (SteeringWheelAngle) – ROS message containing steering wheel angle measurement, (as_can_msgs.msg.SteeringWheelAngle).

try_aligning_preloaded_map_via_gps()

Try to align preloaded map via gps measurements.

Preload filter must be set and alignment via gps configured. Also GPS measurements must exist and transformer for GPS measurements exist. If so, translate and rotate preloaded map and afterwards load preloaded map into main filter.

try_aligning_preloaded_map_via_main_filter()

Try to align preloaded map via the perceived and tracked cones in the main filter.

Preload filter must be set, alignment configured but not via gps. Also atleast a configured amount of landmarks must be perceived excluding big orange cones. If so, alignment event is set so other thread can align preloaded map.

try_preload_map()

Try preloading map (when activated) from previously saved json file.

Preloadable maps lie under as_ros/src/slam/plots/maps/<test_day>/<track_layout>/<file_name>.json. Filename can be:

1. manual.json, being created by Creating Tracks from drone footage or Generating a Map

2. ground_truth.json, being created by Creating Tracks from drone footage

3. map_<number>.json with number being auto incrementing. The highest number will have the highest priority.

Tracks are loaded according to this order. When a filename does not exist, the next possible filename in order is chosen.

An extra UKF, called peload filter, is used to load the preloaded map into. If alignment of the preloaded map is deactived, the preloaded map saved in the preloading filter is afterwards saved into the main filter. Also, if necessary and possible, the preloaded centerpoints are published.

update_landmark_update_strategy(strategy: LandmarkUpdateStrategy)

Update the LandmarkUpdateStrategy of the local mapping sensor of the slam.SLAM.main_filter.

Parameters:

strategy (helpers.LandmarkUpdateStrategy) – new LandmarkUpdateStrategy-

wheelspeed_front_callback(wheelspeed_front: Wheelspeed)

Buffer front wheelspeed measurements.

Buffers new measurements to sensors.wheelspeed.CUREDiffWheelspeed.ros_z_array of the respective sensor (slam.SLAM.wheelspeed_fl and slam.SLAM.wheelspeed_fr).

Parameters:

wheelspeed_front (Wheelspeed) – ROS message containing the front wheelspeed measurements, (as_can_msgs.msg.Wheelspeed).

wheelspeed_rear_callback(wheelspeed_rear: Wheelspeed)

Buffer rear wheelspeed measurements.

Buffers new measurements to sensors.wheelspeed.CUREDiffWheelspeed.ros_z_array of the respective sensor (slam.SLAM.wheelspeed_rl and slam.SLAM.wheelspeed_rr).

Parameters:

msg (Wheelspeed) – ROS message containing the rear wheelspeed measurements, (as_can_msgs.msg.Wheelspeed).