parent
236dffe400
commit
6dba364556
2 changed files with 0 additions and 100 deletions
@ -1,99 +0,0 @@ |
||||
#pragma once |
||||
|
||||
#include <eigen3/Eigen/Dense> |
||||
#include <deque> |
||||
#include <fstream> |
||||
#include <memory> |
||||
#include <map> |
||||
#include <string> |
||||
|
||||
#include "cereal/messaging/messaging.h" |
||||
#include "common/transformations/coordinates.hpp" |
||||
#include "common/transformations/orientation.hpp" |
||||
#include "common/params.h" |
||||
#include "common/swaglog.h" |
||||
#include "common/timing.h" |
||||
#include "common/util.h" |
||||
|
||||
#include "system/sensord/sensors/constants.h" |
||||
#define VISION_DECIMATION 2 |
||||
#define SENSOR_DECIMATION 10 |
||||
#include "selfdrive/locationd/models/live_kf.h" |
||||
|
||||
#define POSENET_STD_HIST_HALF 20 |
||||
|
||||
enum LocalizerGnssSource { |
||||
UBLOX, QCOM |
||||
}; |
||||
|
||||
class Localizer { |
||||
public: |
||||
Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX); |
||||
|
||||
int locationd_thread(); |
||||
|
||||
void reset_kalman(double current_time = NAN); |
||||
void reset_kalman(double current_time, const Eigen::VectorXd &init_orient, const Eigen::VectorXd &init_pos, const Eigen::VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R); |
||||
void reset_kalman(double current_time, const Eigen::VectorXd &init_x, const MatrixXdr &init_P); |
||||
void finite_check(double current_time = NAN); |
||||
void time_check(double current_time = NAN); |
||||
void update_reset_tracker(); |
||||
bool is_gps_ok(); |
||||
bool critical_services_valid(const std::map<std::string, double> &critical_services); |
||||
bool is_timestamp_valid(double current_time); |
||||
void determine_gps_mode(double current_time); |
||||
bool are_inputs_ok(); |
||||
void observation_timings_invalid_reset(); |
||||
|
||||
void build_pose_message( |
||||
MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool msgValid); |
||||
void build_live_pose(cereal::LivePose::Builder& fix); |
||||
|
||||
Eigen::VectorXd get_position_geodetic(); |
||||
Eigen::VectorXd get_state(); |
||||
Eigen::VectorXd get_stdev(); |
||||
|
||||
void handle_msg_bytes(const char *data, const size_t size); |
||||
void handle_msg(const cereal::Event::Reader& log); |
||||
void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log); |
||||
void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset); |
||||
void handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log); |
||||
void handle_car_state(double current_time, const cereal::CarState::Reader& log); |
||||
void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log); |
||||
void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log); |
||||
|
||||
void input_fake_gps_observations(double current_time); |
||||
|
||||
private: |
||||
std::unique_ptr<LiveKalman> kf; |
||||
|
||||
Eigen::VectorXd calib; |
||||
MatrixXdr device_from_calib; |
||||
MatrixXdr calib_from_device; |
||||
bool calibrated = false; |
||||
|
||||
double car_speed = 0.0; |
||||
double last_reset_time = NAN; |
||||
std::deque<double> posenet_stds; |
||||
|
||||
std::unique_ptr<LocalCoord> converter; |
||||
|
||||
int64_t unix_timestamp_millis = 0; |
||||
double reset_tracker = 0.0; |
||||
bool gps_mode = false; |
||||
double first_valid_log_time = NAN; |
||||
double ttff = NAN; |
||||
double last_gps_msg = 0; |
||||
LocalizerGnssSource gnss_source; |
||||
bool observation_timings_invalid = false; |
||||
std::map<std::string, double> observation_values_invalid; |
||||
bool standstill = true; |
||||
int32_t orientation_reset_count = 0; |
||||
float gps_std_factor; |
||||
float gps_variance_factor; |
||||
float gps_vertical_variance_factor; |
||||
double gps_time_offset; |
||||
Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std
|
||||
|
||||
void configure_gnss_source(const LocalizerGnssSource &source); |
||||
}; |
Loading…
Reference in new issue