mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-14 19:44:05 +08:00
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
tools/sim/bridge.py
+20 -14
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))