From d85773a06be7adeb17a21fc031f8026b5a1f03d6 Mon Sep 17 00:00:00 2001 From: Theresa Johansson Date: Thu, 30 Jul 2026 15:39:42 +0200 Subject: [PATCH] fix acceleration control not scaling with time elapsed --- src/svea_core/svea_core/interfaces/actuation.py | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/svea_core/svea_core/interfaces/actuation.py b/src/svea_core/svea_core/interfaces/actuation.py index 6b9c04b9..00388ea8 100644 --- a/src/svea_core/svea_core/interfaces/actuation.py +++ b/src/svea_core/svea_core/interfaces/actuation.py @@ -1,6 +1,7 @@ """ Author: Frank Jiang, Tobias Bolin """ +import time from math import pi, isnan from typing import Optional @@ -59,6 +60,7 @@ def __init__(self, rate=20, use_acceleration=False, highgear=False, difflock=Fal self.velocity_percent = 0.0 self.highgear = highgear self.difflock = difflock + self._last_accel_time = None def on_startup(self): self.node.get_logger().info("Starting Actuation Interface Node...") @@ -117,8 +119,18 @@ def __send_steering(self, steering): def __send_velocity(self, velocity): self.velocity_percent = velocity - def __send_acceleration(self, acceleration): - acc_percent = self._speed_to_percent(acceleration) + def __send_acceleration(self, acceleration): + # Get time dt elapsed since last acceleration command + now = time.monotonic() + if self._last_accel_time is None: + dt = 0.0 + else: + dt = now - self._last_accel_time + self._last_accel_time = now + + # acceleration gives slope of speed so velocity change (acc_percent) + # is acceleration * (elapsed time) + acc_percent = self._speed_to_percent(acceleration) * dt vel_percent = self._speed_clip(self.velocity_percent + acc_percent) self.__send_velocity(vel_percent)