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)