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 |
||||||
ubloxd_test |
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