diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp index 595bdf0ddd20cd91ee5452ea168fcb670821a17a..0cbf7e730f56186618f92c0a74be2b5dc5ad0f5a 100644 --- a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp +++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp @@ -92,9 +92,15 @@ armarx::ObstacleAvoidingPlatformUnitHelper::update() m_waypoint_changed = true; } + // Only call `moveTo` on platform unit if the target actually changed, as `update` is designed + // to be called within a high-frequency loop. Prevents constantly invalidating the buffers in + // platform unit. if (m_waypoint_changed) { - // Get thresholds depending on waypoint or final target (last waypoint). + // Use reached-thresholds regardless of whether this is the final target or just a waypoint. + // The near-thresholds are more unconstrained as the reached-thresholds, so this helper will + // change the position target before the unit will actually reach it, preventing slow downs + // or stops. const float pos_thresh = m_cfg.pos_reached_threshold; const float ori_thresh = m_cfg.ori_reached_threshold;