diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp index a847c5f16420610b778b0c1907b9ecc6fbb20172..f741eefd000805e2dba683fc46fded402052d995 100644 --- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp +++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp @@ -1190,15 +1190,18 @@ void KinematicUnitWidgetController::reportJointMotorTemperatures(const NameValue void KinematicUnitWidgetController::reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current&) { - if (!aValueChanged && reportedJointStatuses.size() > 0) + boost::recursive_mutex::scoped_lock lock(mutexNodeSet); + if (aValueChanged && jointCurrents.size() > 0) { - return; + jointCurrentHistory.push_back(std::pair<Ice::Long, NameValueMap>(timestamp, jointCurrents)); } - boost::recursive_mutex::scoped_lock lock(mutexNodeSet); - reportedJointStatuses = jointStatuses; + while (jointCurrentHistory.back().first - jointCurrentHistory.front().first > historyTime && jointCurrentHistory.size() > 1) + { + jointCurrentHistory.pop_front(); + } - emit jointStatusesReported(); + emit jointCurrentsReported(); }