mawei/onepilot
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

sim: fix sensor freq and timestamps (#25937)

Browse Source 
* sim: fix sensor freq and timestamps

* 100hz

* fix that too
old-commit-hash: 5d33199905cbf9d9b45ef722a40530b08d5cecf4
This commit is contained in:
Adeeb Shihadeh
2022-09-29 22:36:09 -07:00
committed by GitHub
parent d7acc9fe75
commit 1cb4cdab24
2 changed files with 22 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
selfdrive/locationd/locationd.cc
View File

@@ -499,7 +499,7 @@ int Localizer::locationd_thread() {
}
const std::initializer_list<const char *> service_list = {gps_location_socket,
"cameraOdometry", "liveCalibration", "carState", "carParams",
"accelerometer", "gyroscope", "magnetometer"};
"accelerometer", "gyroscope"};
PubMaster pm({"liveLocationKalman"});
// TODO: remove carParams once we're always sending at 100Hz
@@ -526,7 +526,7 @@ int Localizer::locationd_thread() {
if (sm.updated(trigger_msg)) {
bool inputsOK = sm.allAliveAndValid();
bool gpsOK = this->isGpsOK();
bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope", "magnetometer"});
bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"});
MessageBuilder msg_builder;
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);

34
tools/sim/bridge.py
View File

@@ -122,21 +122,26 @@ class Camerad:
pm.send(pub_type, dat)
def imu_callback(imu, vehicle_state):
vehicle_state.bearing_deg = math.degrees(imu.compass)
dat = messaging.new_message('accelerometer')
dat.accelerometer.sensor = 4
dat.accelerometer.type = 0x1
dat.accelerometer.init('acceleration')
dat.accelerometer.acceleration.v = [imu.accelerometer.x, imu.accelerometer.y, imu.accelerometer.z]
pm.send('accelerometer', dat)
# send 5x since 'sensor_tick' doesn't seem to work. limited by the world tick?
for _ in range(5):
vehicle_state.bearing_deg = math.degrees(imu.compass)
dat = messaging.new_message('accelerometer')
dat.accelerometer.sensor = 4
dat.accelerometer.type = 0x1
dat.accelerometer.timestamp = dat.logMonoTime # TODO: use the IMU timestamp
dat.accelerometer.init('acceleration')
dat.accelerometer.acceleration.v = [imu.accelerometer.x, imu.accelerometer.y, imu.accelerometer.z]
pm.send('accelerometer', dat)
# copied these numbers from locationd
dat = messaging.new_message('gyroscope')
dat.gyroscope.sensor = 5
dat.gyroscope.type = 0x10
dat.gyroscope.init('gyroUncalibrated')
dat.gyroscope.gyroUncalibrated.v = [imu.gyroscope.x, imu.gyroscope.y, imu.gyroscope.z]
pm.send('gyroscope', dat)
# copied these numbers from locationd
dat = messaging.new_message('gyroscope')
dat.gyroscope.sensor = 5
dat.gyroscope.type = 0x10
dat.gyroscope.timestamp = dat.logMonoTime # TODO: use the IMU timestamp
dat.gyroscope.init('gyroUncalibrated')
dat.gyroscope.gyroUncalibrated.v = [imu.gyroscope.x, imu.gyroscope.y, imu.gyroscope.z]
pm.send('gyroscope', dat)
time.sleep(0.01)
def panda_state_function(vs: VehicleState, exit_event: threading.Event):
@@ -351,6 +356,7 @@ class CarlaBridge:
# re-enable IMU
imu_bp = blueprint_library.find('sensor.other.imu')
imu_bp.set_attribute('sensor_tick', '0.01')
imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle)
imu.listen(lambda imu: imu_callback(imu, vehicle_state))