3.1. Utilities Messages¶
3.1.1. Perception Messages¶
- message utilities/BoundingBox
- Parameters:
probability (
float64
) – Probability that the perceived cone is of this color.id (
int16
) – ID (color) of the perceived cone. 0: yellow, 1: blue, 2: small orange, 3: big orange.used_mask (
bool
) – Whether the mask of the cone was used to determine the cone topx_cone_top (
int16
) – x point of top of the perceived cone masky_cone_top (
int16
) – y point of top of the perceived cone maskxmin (
int64
) – X coordinate of the upper left corner of the bounding box.ymin (
int64
) – Y coordinate of the upper left corner of the bounding box.xmax (
int64
) – X coordinate of the lower right corner of the bounding box.ymax (
int64
) – Y coordinate of the lower right corner of the bounding box.Class (
string
) – String representation of the color of the perceived cone.mask (
sensor_msgs/Image
) – Mask of the perceived cone.
Message containing a bounding box for a perceived cone in an image.
- message utilities/BoundingBoxes
- Parameters:
header (
Header
) – Header containing the time the bounding boxes have been calculated at.image_header (
Header
) – Header of the processed image containing the time the image has been recorded at.lidar_header (
Header
) – Header of the lidar pointcloud to match with the image.processing_time (
float64
) – Processing time for one imagebounding_boxes (
utilities/BoundingBox[]
) – Bounding boxes perceived in the image.
Message containing all bounding boxes for an processed image by the trt inference.
3.1.2. Mapping Messages¶
- message utilities/ConePosition
- Parameters:
x (
float64
) – X position of the cone.y (
float64
) – Y position of the cone.id (
int16
) – ID (color) of the cone.probability (
float64
) – Probability that the cone is of this color.is_lidar (
bool
) – LiDAR was used to determine position
Message containing a position, id and probability for a cone.
- message utilities/ConeList
- Parameters:
header (
Header
) – Header containing the time the cone positions have been calculated at.image_header (
Header
) – Header of the image the cones have been perceived in containing the time the image has been recorded at.lidar_header (
Header
) – Header of the image the cones have been perceived in containing the time the image has been recorded at.cones (
utilities/ConePosition[]
) – List of cone positions.processing_time (
float64
) – Time needed for the calculationlidar_processing_time (
float64
) – Time needed for the lidar transformationsensor_time_delta (
float64
) – Time needed for the lidar transformation
Message containing a list of cone positions.
3.1.3. SLAM Messages¶
- message utilities/VehiclePose
- Parameters:
header (
Header
) – Header containing the time the can message has been received at.x (
float64
) – X coordinate of global vehicle position.y (
float64
) – Y coordinate of global vehicle position.v_x (
float64
) – Velocity in x-axis direction of vehicle.psi (
float64
) – Global heading of vehicle.P (
float64[16]
) – Covariance of the vehicle pose.
Message containing a vehicle pose: position, velocity and heading.
- message utilities/ConePositionWithCovariance
- Parameters:
x (
float64
) – X position of the cone.y (
float64
) – Y position of the cone.id (
int16
) – ID (color) of the cone.perceived_n (
uint16
) – Number of times the cone has already been perceivedcovariance (
float64[4]
) – Position covariance of the cone.
Message containing a position, id and position covariance for a cone.
- message utilities/ConeListWithCovariance
- Parameters:
header (
Header
) – Header containing the latest image header the cones are based on.cones (
utilities/ConePositionWithCovariance[]
) – List of cones with position, color and covariance.closed_track (
bool
) – Flag whether the tracked landmarks form a closed track.
Message containing a list of cone positions, colors and covariance.
- message utilities/DisablePerception
- Parameters:
header (
Header
) – Header containing the time the message has been created at.disable_perception (
bool
) – Flag to indicate whether the perception should be enabled or disabled.
Message containing flag to disable the perception when not needed anymore for SLAM.
- message utilities/PredictedMeasurements
- Parameters:
header (
Header
) – Time the kalman filter has been executed for.wheelspeed_fr (
float64[3]
) – Predicted measurement, true measurement and it’s uncertainty for the front right wheelspeed.wheelspeed_fl (
float64[3]
) – Predicted measurement, true measurement and it’s uncertainty for the front left wheelspeed.wheelspeed_rr (
float64[3]
) – Predicted measurement, true measurement and it’s uncertainty for the rear right wheelspeed.wheelspeed_rl (
float64[3]
) – Predicted measurement, true measurement and it’s uncertainty for the rear left wheelspeed.gps_heading (
float64[3]
) – Predicted measurement, true measurement and it’s uncertainty for the gps heading.gps_speed (
float64[3]
) – Predicted measurement, true measurement and it’s uncertainty for the gps speed.gps_x (
float64[3]
) – Predicted measurement, true measurement and it’s uncertainty for the gps x position.gps_y (
float64[3]
) – Predicted measurement, true measurement and it’s uncertainty for the gps y position.u (
float64[3]
) – Control input [Steering Wheel Angle, Acceration in x direction and gyro around z axis]observed_landmarks (
utilities/ConePosition[]
) – List of true observed landmarks.predicted_observed_landmarks (
utilities/ConePosition[]
) – List of predicted observed landmarks.observable_landmarks (
utilities/ConePosition[]
) – List of observable landmarks.mapping (
int16[]
) – Mapping between observed landmarks and mapped landmarksindividual_compatibility (
bool[]
) – Compatibility between observed and observable landmarks (observable x observed)used_sensors (
string[]
) – List of used sensors during update step.landmark_association_nodes_explored_n (
uint32
) – Number of explored nodes during landmark associationlandmark_association_compared_associations_n (
uint32
) – Number of compared associations during landmark associationcluster_sizes (
uint16[]
) – Number of landmarks in each clusterexecution_time (
float64
) – Time it took to execute the kalman filterlocal_msg_usage_delay (
float64
) – Time it took from arrival of LocalMapping message until it was usedupdate_unscented_transform_time_lm (
float64
) – Time it took to execute the corresponding functions in secondsupdate_calculate_sigma_f_points_time_lm (
float64
) – Time it took to execute the corresponding functions in secondsdata_association_time (
float64
) – Time it took to execute the corresponding functions in seconds
Message used to debug the SLAM algorithm regarding the predicted and true measurements and their uncertainty.
3.1.4. Path Planning Messages¶
- message utilities/CenterPoints
- Parameters:
header (
Header
) – Header containing the time the centerpoints have been calculated at.image_header (
Header
) – Header of the image the track is based on containing the time the image has been recorded at.x (
float64[]
) – X coordinates of the computed centerpoints.y (
float64[]
) – Y coordinates of the computed centerpoints.left_track_width (
float64[]
) – Left track width of the computed centerpoints.right_track_width (
float64[]
) – Right track width of the computed centerpoints.closed_track (
bool
) – Flag whether the centerpoints form a closed track.vehicle_pose (
utilities/VehiclePose
) – Vehicle pose for which the centerpoints have been computed for.strategy (
int32[]
) – Strategy used to compute the centerpoints.elapsed_time (
float64
) – Runtime of the centerpoint computation.
message containing the coordinates of the centerpoints of the perceived and filtered track and the matching right and left track width.
3.1.5. Motion Planning Messages¶
- message utilities/TrajectoryPoints
- Parameters:
header (
Header
) – Header containing the time the trajectory has been calculated at.image_header (
Header
) – Header of the image the centerpoints are based on.pose_header (
Header
) – Header of the vehicle pose the trajectory has been calculated for.x (
float64[]
) – X coordinate of the planned trajectory.y (
float64[]
) – Y coordinate of the planned trajectory.v (
float64[]
) – Velocity matching to the coordinates of the trajectory.closed (
bool
) – Flag indicating whether the trajectory is closed.lap_time (
float64
) – Lap time of the planned closed trajectory in seconds.local_planning_calculation_time (
float64
) – Time the local motion planning needed for calculation in seconds.global_planning_calculation_time (
float64
) – Time the global motion planning needed for calculation in seconds.
Message containing the planned trajectory.
3.1.6. Control Messages¶
- message utilities/PredictedStates
- Parameters:
header (
Header
) – Header containing the time the predicted states have been calculated at.pose_header (
Header
) – Header of the vehicle pose the predicted states has been calculated for.image_header (
Header
) – Header of the image the trajectory has been calculated for.t (
float64[]
) – Relative time of the predicted vehicle states.x_pos (
float64[]
) – X coordinates of the predicted vehicle states.y_pos (
float64[]
) – Y coordinates of the predicted vehicle states.psi (
float64[]
) – Global heading of the predicted vehicle states.v_x (
float64[]
) – Velocity in x-axis direction of the predicted vehicle states.steering_wheel_angle (
float64[]
) – Steering wheel angle of the predicted vehicle states.delta_rate (
float64[]
) – Ackermann rate of the predicted control inputsa_x (
float64[]
) – Accelerations in x direction of the predicted control inputstorque (
float64[]
) – Necessary motor torque request to reach the respective accelerationservice_brake_pressure (
float64[]
) – Necessary service brake pressure request to reach the respective acceleration
Message containing the predicted states of the model predictice controller of the control module.
- message utilities/MPCStatistics
- Parameters:
header (
Header
) – Header containing the time the control output has been calculated at.iterations (
uint16
) – Number of iterations.OPTIMAL_SOLUTION_FOUND=1 (
int8
) – Exitflag of the solver: https://forces.embotech.com/Documentation/high_level_interface/index.html#exitflags-and-quality-of-the-resultMAX_ITERATION_REACHED=0 (
int8
) – Exitflag of the solver: https://forces.embotech.com/Documentation/high_level_interface/index.html#exitflags-and-quality-of-the-resultWRONG_NUMBER_OFINEQUALITIES=-4 (
int8
) – Exitflag of the solver: https://forces.embotech.com/Documentation/high_level_interface/index.html#exitflags-and-quality-of-the-resultMATRIX_FACTORIZATION_ERROR=-5 (
int8
) – Exitflag of the solver: https://forces.embotech.com/Documentation/high_level_interface/index.html#exitflags-and-quality-of-the-resultNAN_OR_INF_IN_FUNCTION=-6 (
int8
) – Exitflag of the solver: https://forces.embotech.com/Documentation/high_level_interface/index.html#exitflags-and-quality-of-the-resultSOLVER_COULD_NOT_PROCEED=-7 (
int8
) – Exitflag of the solver: https://forces.embotech.com/Documentation/high_level_interface/index.html#exitflags-and-quality-of-the-resultQP_SOLVER_COULD_NOT_PROCEED=-8 (
int8
) – Exitflag of the solver: https://forces.embotech.com/Documentation/high_level_interface/index.html#exitflags-and-quality-of-the-resultNAN_OR_INF_IN_FUNCTION_OR_DERIVATIVES=-10 (
int8
) – Exitflag of the solver: https://forces.embotech.com/Documentation/high_level_interface/index.html#exitflags-and-quality-of-the-resultINVALID_PROBLEM_PARAMETERS=-11 (
int8
) – Exitflag of the solver: https://forces.embotech.com/Documentation/high_level_interface/index.html#exitflags-and-quality-of-the-resultLICENSE_ERROR=-100 (
int8
) – Exitflag of the solver: https://forces.embotech.com/Documentation/high_level_interface/index.html#exitflags-and-quality-of-the-resultexitflag (
int8
) – Exitflag of the solver: https://forces.embotech.com/Documentation/high_level_interface/index.html#exitflags-and-quality-of-the-resultsolve_time (
float64
) – Solvetime in secondsfunction_evaluation_time (
float64
) – Evaluation time of all user functions in secondsobjective (
float64
) – Value of objective function
Message containing the statistics for the solver statistics of the mpc for a prediction step