diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp index ebbc4f01ad67450b8cc52a6900a7a03609097af2..94109774002f45404c6f46b26708de87882d5e60 100644 --- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp +++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp @@ -61,7 +61,10 @@ namespace armarx { historyLength = getProperty<int>("HistoryLength").getValue(); - history.clear(); + { + boost::unique_lock<SharedMutex> lock(historyMutex); + history.clear(); + } relativeRobotFile = getProperty<std::string>("RobotFileName").getValue(); @@ -210,7 +213,6 @@ namespace armarx RobotStateConfig config; if (!interpolate(timestamp, config)) { - ARMARX_WARNING << "Could not find or interpolate a robot state for time " << IceUtil::Time::microSeconds(timestamp).toDateTime(); return NameValueMap(); } return config.jointMap; @@ -222,7 +224,6 @@ namespace armarx RobotStateConfig config; if (!interpolate(timestamp, config)) { - ARMARX_WARNING << "Could not find or interpolate a robot state for time " << IceUtil::Time::microSeconds(timestamp).toDateTime(); return RobotStateConfig(); } return config; @@ -242,6 +243,7 @@ namespace armarx } else if (it == history.end()) { + ARMARX_WARNING << "Could not find or interpolate a robot state for time " << IceUtil::Time::microSeconds(time).toDateTime() << " - oldest available value: " << IceUtil::Time::microSeconds(history.begin()->first).toDateTime(); return false; } else