mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-18 07:42:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Handle posenet and sensor alerts in locationd (#1541)

Browse Source 
* handle posenet and senor alerts in locationd

* defaults now set in capnp file

* Cleanup c++ version of params learner

* update ref commit
old-commit-hash: d099e09fb77544eb00d13ba9aea34a0a3374a52e
This commit is contained in:
Willem Melching
2020-05-19 16:45:20 -07:00
committed by GitHub
parent fb7e47e2fb
commit 6e41973c3c
8 changed files with 21 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+3 -5
View File
@@ -50,7 +50,7 @@ class Controls:
self.sm = sm
if self.sm is None:
self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', \
'dMonitoringState', 'plan', 'pathPlan'])
'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'])
self.can_sock = can_sock
if can_sock is None:
@@ -123,8 +123,6 @@ class Controls:
self.current_alert_types = []
self.sm['liveCalibration'].calStatus = Calibration.INVALID
self.sm['pathPlan'].sensorValid = True
self.sm['pathPlan'].posenetValid = True
self.sm['thermal'].freeSpace = 1.
self.sm['dMonitoringState'].events = []
self.sm['dMonitoringState'].awarenessStatus = 1.
@@ -201,11 +199,11 @@ class Controls:
self.events.add(EventName.commIssue)
if not self.sm['pathPlan'].mpcSolutionValid:
self.events.add(EventName.plannerError)
if not self.sm['pathPlan'].sensorValid and os.getenv("NOSENSOR") is None:
if not self.sm['liveLocationKalman'].inputsOK and os.getenv("NOSENSOR") is None:
self.events.add(EventName.sensorDataInvalid)
if not self.sm['pathPlan'].paramsValid:
self.events.add(EventName.vehicleModelInvalid)
if not self.sm['pathPlan'].posenetValid:
if not self.sm['liveLocationKalman'].posenetOK:
self.events.add(EventName.posenetInvalid)
if not self.sm['plan'].radarValid:
self.events.add(EventName.radarFault)
selfdrive/controls/lib/pathplanner.py
-2
View File
@@ -207,8 +207,6 @@ class PathPlanner():
plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage)
plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid)
plan_send.pathPlan.posenetValid = bool(sm['liveParameters'].posenetValid)
plan_send.pathPlan.desire = desire
plan_send.pathPlan.laneChangeState = self.lane_change_state
selfdrive/locationd/locationd.py
+15 -2
View File
@@ -57,6 +57,10 @@ class Localizer():
self.calibrated = 0
self.H = get_H()
self.posenet_invalid_count = 0
self.posenet_speed = 0
self.car_speed = 0
@staticmethod
def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov):
predicted_std = np.sqrt(np.diagonal(predicted_cov))
@@ -141,6 +145,12 @@ class Localizer():
def liveLocationMsg(self, time):
fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P)
if abs(self.posenet_speed - self.car_speed) > max(0.4 * self.car_speed, 5.0):
self.posenet_invalid_count += 1
else:
self.posenet_invalid_count = 0
fix.posenetOK = self.posenet_invalid_count < 4
#fix.gpsWeek = self.time.week
#fix.gpsTimeOfWeek = self.time.tow
fix.unixTimestampMillis = self.unix_timestamp_millis
@@ -198,12 +208,12 @@ class Localizer():
self.update_kalman(current_time, ObservationKind.ECEF_POS, ecef_pos, R=ecef_pos_R)
self.update_kalman(current_time, ObservationKind.ECEF_VEL, ecef_vel, R=ecef_vel_R)
def handle_car_state(self, current_time, log):
self.speed_counter += 1
if self.speed_counter % SENSOR_DECIMATION == 0:
self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, [log.vEgo])
self.car_speed = abs(log.vEgo)
if log.vEgo == 0:
self.update_kalman(current_time, ObservationKind.NO_ROT, [0, 0, 0])
@@ -218,6 +228,7 @@ class Localizer():
np.concatenate([rot_device, 10*rot_device_std]))
trans_device = self.device_from_calib.dot(log.trans)
trans_device_std = self.device_from_calib.dot(log.transStd)
self.posenet_speed = np.linalg.norm(trans_device)
self.update_kalman(current_time,
ObservationKind.CAMERA_ODO_TRANSLATION,
np.concatenate([trans_device, 10*trans_device_std]))
@@ -263,7 +274,7 @@ class Localizer():
def locationd_thread(sm, pm, disabled_logs=[]):
if sm is None:
socks = ['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration']
socks = ['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration', 'carState']
sm = messaging.SubMaster(socks)
if pm is None:
pm = messaging.PubMaster(['liveLocationKalman'])
@@ -293,6 +304,8 @@ def locationd_thread(sm, pm, disabled_logs=[]):
msg.logMonoTime = t
msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9)
msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid()
pm.send('liveLocationKalman', msg)
selfdrive/locationd/locationd_yawrate.cc
-6
View File
@@ -32,7 +32,6 @@ void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double
void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time) {
for (cereal::SensorEventData::Reader sensor_event : sensor_events){
if (sensor_event.getSensor() == 5 && sensor_event.getType() == 16) {
sensor_data_time = current_time;
double meas = -sensor_event.getGyroUncalibrated().getV()[0];
update_state(C_gyro, R_gyro, current_time, meas);
}
@@ -43,16 +42,11 @@ void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odo
double R = pow(5 * camera_odometry.getRotStd()[2], 2);
double meas = camera_odometry.getRot()[2];
update_state(C_posenet, R, current_time, meas);
auto trans = camera_odometry.getTrans();
posenet_speed = sqrt(trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2]);
camera_odometry_time = current_time;
}
void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) {
steering_angle = controls_state.getAngleSteers() * DEGREES_TO_RADIANS;
car_speed = controls_state.getVEgo();
controls_state_time = current_time;
}
selfdrive/locationd/locationd_yawrate.h
-4
View File
@@ -25,11 +25,7 @@ public:
Eigen::Matrix2d P;
double steering_angle = 0;
double car_speed = 0;
double posenet_speed = 0;
double prev_update_time = -1;
double controls_state_time = -1;
double sensor_data_time = -1;
double camera_odometry_time = -1;
Localizer();
void handle_log(cereal::Event::Reader event);
selfdrive/locationd/paramsd.cc
+1 -15
View File
@@ -116,23 +116,12 @@ int main(int argc, char *argv[]) {
localizer.handle_log(event);
auto which = event.which();
// Throw vision failure if posenet and odometric speed too different
if (which == cereal::Event::CAMERA_ODOMETRY){
if (std::abs(localizer.posenet_speed - localizer.car_speed) > std::max(0.4 * localizer.car_speed, 5.0)) {
posenet_invalid_count++;
} else {
posenet_invalid_count = 0;
}
} else if (which == cereal::Event::CONTROLS_STATE){
if (which == cereal::Event::CONTROLS_STATE){
save_counter++;
double yaw_rate = -localizer.x[0];
bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle);
// TODO: Fix in replay
double sensor_data_age = localizer.controls_state_time - localizer.sensor_data_time;
double camera_odometry_age = localizer.controls_state_time - localizer.camera_odometry_time;
double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao;
double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao;
@@ -143,13 +132,10 @@ int main(int argc, char *argv[]) {
live_params.setValid(valid);
live_params.setYawRate(localizer.x[0]);
live_params.setGyroBias(localizer.x[1]);
live_params.setSensorValid(sensor_data_age < 5.0);
live_params.setAngleOffset(angle_offset_degrees);
live_params.setAngleOffsetAverage(angle_offset_average_degrees);
live_params.setStiffnessFactor(learner.x);
live_params.setSteerRatio(learner.sR);
live_params.setPosenetSpeed(localizer.posenet_speed);
live_params.setPosenetValid((posenet_invalid_count < 4) && (camera_odometry_age < 5.0));
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
selfdrive/test/process_replay/process_replay.py
+1 -1
View File
@@ -202,7 +202,7 @@ CONFIGS = [
proc_name="controlsd",
pub_sub={
"can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"],
"thermal": [], "health": [], "liveCalibration": [], "dMonitoringState": [], "plan": [], "pathPlan": [], "gpsLocation": [],
"thermal": [], "health": [], "liveCalibration": [], "dMonitoringState": [], "plan": [], "pathPlan": [], "gpsLocation": [], "liveLocationKalman": [],
"model": [],
},
ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"],
selfdrive/test/process_replay/ref_commit
+1 -1
View File
@@ -1 +1 @@
76e577b86d113139167275b4a7379f3591abfa02
59bd32350139796784708f51c233d37c18f33e6e