Vehicle Researcher
6 years ago
parent
cd98235644
commit
94053536b4
61 changed files with 1434 additions and 973 deletions
File diff suppressed because it is too large
Load Diff
Binary file not shown.
@ -1 +1 @@ |
||||
#define COMMA_VERSION "0.6-release" |
||||
#define COMMA_VERSION "0.6.1-release" |
||||
|
||||
@ -1,3 +1,4 @@ |
||||
ubloxd |
||||
ubloxd_test |
||||
params_learner |
||||
params_learner |
||||
paramsd |
||||
@ -0,0 +1,34 @@ |
||||
#pragma once |
||||
|
||||
#include <eigen3/Eigen/Dense> |
||||
#include "cereal/gen/cpp/log.capnp.h" |
||||
|
||||
#define DEGREES_TO_RADIANS 0.017453292519943295 |
||||
|
||||
class Localizer |
||||
{ |
||||
Eigen::Matrix2d A; |
||||
Eigen::Matrix2d I; |
||||
Eigen::Matrix2d Q; |
||||
Eigen::Matrix2d P; |
||||
Eigen::Matrix<double, 1, 2> C_posenet; |
||||
Eigen::Matrix<double, 1, 2> C_gyro; |
||||
|
||||
double R_gyro; |
||||
|
||||
void update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas); |
||||
void handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time); |
||||
void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time); |
||||
void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time); |
||||
|
||||
public: |
||||
Eigen::Vector2d x; |
||||
double steering_angle = 0; |
||||
double car_speed = 0; |
||||
double prev_update_time = -1; |
||||
double controls_state_time = -1; |
||||
double sensor_data_time = -1; |
||||
|
||||
Localizer(); |
||||
cereal::Event::Which handle_log(const unsigned char* msg_dat, size_t msg_size); |
||||
}; |
||||
@ -0,0 +1,174 @@ |
||||
#include <czmq.h> |
||||
#include <capnp/message.h> |
||||
#include <capnp/serialize-packed.h> |
||||
|
||||
#include "locationd_yawrate.h" |
||||
#include "cereal/gen/cpp/log.capnp.h" |
||||
|
||||
#include "common/swaglog.h" |
||||
#include "common/messaging.h" |
||||
#include "common/params.h" |
||||
#include "common/timing.h" |
||||
#include "params_learner.h" |
||||
#include "json11.hpp" |
||||
|
||||
const int num_polls = 3; |
||||
|
||||
int main(int argc, char *argv[]) { |
||||
auto ctx = zmq_ctx_new(); |
||||
auto controls_state_sock = sub_sock(ctx, "tcp://127.0.0.1:8007"); |
||||
auto sensor_events_sock = sub_sock(ctx, "tcp://127.0.0.1:8003"); |
||||
auto camera_odometry_sock = sub_sock(ctx, "tcp://127.0.0.1:8066"); |
||||
|
||||
auto live_parameters_sock = zsock_new_pub("@tcp://*:8064"); |
||||
assert(live_parameters_sock); |
||||
auto live_parameters_sock_raw = zsock_resolve(live_parameters_sock); |
||||
|
||||
int err; |
||||
Localizer localizer; |
||||
|
||||
zmq_pollitem_t polls[num_polls] = {{0}}; |
||||
polls[0].socket = controls_state_sock; |
||||
polls[0].events = ZMQ_POLLIN; |
||||
polls[1].socket = sensor_events_sock; |
||||
polls[1].events = ZMQ_POLLIN; |
||||
polls[2].socket = camera_odometry_sock; |
||||
polls[2].events = ZMQ_POLLIN; |
||||
|
||||
// Read car params
|
||||
char *value; |
||||
size_t value_sz = 0; |
||||
|
||||
LOGW("waiting for params to set vehicle model"); |
||||
while (true) { |
||||
read_db_value(NULL, "CarParams", &value, &value_sz); |
||||
if (value_sz > 0) break; |
||||
usleep(100*1000); |
||||
} |
||||
LOGW("got %d bytes CarParams", value_sz); |
||||
|
||||
// make copy due to alignment issues
|
||||
auto amsg = kj::heapArray<capnp::word>((value_sz / sizeof(capnp::word)) + 1); |
||||
memcpy(amsg.begin(), value, value_sz); |
||||
free(value); |
||||
|
||||
capnp::FlatArrayMessageReader cmsg(amsg); |
||||
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>(); |
||||
|
||||
// Read params from previous run
|
||||
const int result = read_db_value(NULL, "LiveParameters", &value, &value_sz); |
||||
|
||||
std::string fingerprint = car_params.getCarFingerprint(); |
||||
std::string vin = car_params.getCarVin(); |
||||
double sR = car_params.getSteerRatio(); |
||||
double x = 1.0; |
||||
double ao = 0.0; |
||||
|
||||
if (result == 0){ |
||||
auto str = std::string(value, value_sz); |
||||
free(value); |
||||
|
||||
std::string err; |
||||
auto json = json11::Json::parse(str, err); |
||||
if (json.is_null() || !err.empty()) { |
||||
std::string log = "Error parsing json: " + err; |
||||
LOGW(log.c_str()); |
||||
} else { |
||||
std::string new_fingerprint = json["carFingerprint"].string_value(); |
||||
std::string new_vin = json["carVin"].string_value(); |
||||
|
||||
if (fingerprint == new_fingerprint && vin == new_vin) { |
||||
std::string log = "Parameter starting with: " + str; |
||||
LOGW(log.c_str()); |
||||
|
||||
sR = json["steerRatio"].number_value(); |
||||
x = json["stiffnessFactor"].number_value(); |
||||
ao = json["angleOffsetAverage"].number_value(); |
||||
} |
||||
} |
||||
} |
||||
|
||||
ParamsLearner learner(car_params, ao, x, sR, 1.0); |
||||
|
||||
// Main loop
|
||||
int save_counter = 0; |
||||
while (true){ |
||||
int ret = zmq_poll(polls, num_polls, 100); |
||||
|
||||
if (ret == 0){ |
||||
continue; |
||||
} else if (ret < 0){ |
||||
break; |
||||
} |
||||
|
||||
for (int i=0; i < num_polls; i++) { |
||||
if (polls[i].revents) { |
||||
zmq_msg_t msg; |
||||
err = zmq_msg_init(&msg); |
||||
assert(err == 0); |
||||
err = zmq_msg_recv(&msg, polls[i].socket, 0); |
||||
assert(err >= 0); |
||||
// make copy due to alignment issues, will be freed on out of scope
|
||||
auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1); |
||||
memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg)); |
||||
|
||||
auto which = localizer.handle_log((const unsigned char*)amsg.begin(), amsg.size()); |
||||
zmq_msg_close(&msg); |
||||
|
||||
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 angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao; |
||||
double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao; |
||||
|
||||
// Send parameters at 10 Hz
|
||||
if (save_counter % 10 == 0){ |
||||
capnp::MallocMessageBuilder msg; |
||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |
||||
event.setLogMonoTime(nanos_since_boot()); |
||||
auto live_params = event.initLiveParameters(); |
||||
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); |
||||
|
||||
auto words = capnp::messageToFlatArray(msg); |
||||
auto bytes = words.asBytes(); |
||||
zmq_send(live_parameters_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT); |
||||
} |
||||
|
||||
|
||||
// Save parameters every minute
|
||||
if (save_counter % 6000 == 0) { |
||||
json11::Json json = json11::Json::object { |
||||
{"carVin", vin}, |
||||
{"carFingerprint", fingerprint}, |
||||
{"steerRatio", learner.sR}, |
||||
{"stiffnessFactor", learner.x}, |
||||
{"angleOffsetAverage", angle_offset_average_degrees}, |
||||
}; |
||||
|
||||
std::string out = json.dump(); |
||||
write_db_value(NULL, "LiveParameters", out.c_str(), out.length()); |
||||
} |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
zmq_close(controls_state_sock); |
||||
zmq_close(sensor_events_sock); |
||||
zmq_close(camera_odometry_sock); |
||||
zmq_close(live_parameters_sock_raw); |
||||
return 0; |
||||
} |
||||
@ -0,0 +1,53 @@ |
||||
#!/usr/bin/env python |
||||
|
||||
import numpy as np |
||||
import unittest |
||||
|
||||
from selfdrive.car.honda.interface import CarInterface |
||||
from selfdrive.car.honda.values import CAR |
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel |
||||
from selfdrive.locationd.liblocationd_py import liblocationd # pylint: disable=no-name-in-module, import-error |
||||
|
||||
|
||||
class TestParamsLearner(unittest.TestCase): |
||||
def setUp(self): |
||||
|
||||
self.CP = CarInterface.get_params(CAR.CIVIC, {}) |
||||
bts = self.CP.to_bytes() |
||||
|
||||
self.params_learner = liblocationd.params_learner_init(len(bts), bts, 0.0, 1.0, self.CP.steerRatio, 1.0) |
||||
|
||||
def test_convergence(self): |
||||
# Setup vehicle model with wrong parameters |
||||
VM_sim = VehicleModel(self.CP) |
||||
x_target = 0.75 |
||||
sr_target = 14 |
||||
ao_target = -1.0 |
||||
VM_sim.update_params(x_target, sr_target) |
||||
|
||||
# Run simulation |
||||
times = np.arange(0, 10*3600, 0.01) |
||||
angle_offset = np.radians(ao_target) |
||||
steering_angles = np.radians(10 * np.sin(2 * np.pi * times / 100.)) + angle_offset |
||||
speeds = 10 * np.sin(2 * np.pi * times / 1000.) + 25 |
||||
|
||||
for i, t in enumerate(times): |
||||
u = speeds[i] |
||||
sa = steering_angles[i] |
||||
psi = VM_sim.yaw_rate(sa - angle_offset, u) |
||||
liblocationd.params_learner_update(self.params_learner, psi, u, sa) |
||||
|
||||
# Verify learned parameters |
||||
sr = liblocationd.params_learner_get_sR(self.params_learner) |
||||
ao_slow = np.degrees(liblocationd.params_learner_get_slow_ao(self.params_learner)) |
||||
x = liblocationd.params_learner_get_x(self.params_learner) |
||||
self.assertAlmostEqual(x_target, x, places=1) |
||||
self.assertAlmostEqual(ao_target, ao_slow, places=1) |
||||
self.assertAlmostEqual(sr_target, sr, places=1) |
||||
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
||||
Loading…
Reference in new issue