From 53d9f733442dfeba7b1b30b7493348e1b28ba55a Mon Sep 17 00:00:00 2001
From: Raphael Grimm <raphael.grimm@kit.edu>
Date: Wed, 8 Jul 2020 10:23:06 +0200
Subject: [PATCH] Reset history of RobotStateComponent on signal from
SimulatorResetEvent
---
.../components/RobotState/RobotStateComponent.cpp | 13 +++++++++++++
.../components/RobotState/RobotStateComponent.h | 2 +-
source/RobotAPI/interface/core/RobotState.ice | 4 +++-
3 files changed, 17 insertions(+), 2 deletions(-)
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 473ec9a16..271af4bf8 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -156,6 +156,7 @@ namespace armarx
{
ARMARX_WARNING << "Failed to read robot info from file: " << robotFile;
}
+ usingTopic("SimulatorResetEvent");
}
void RobotStateComponent::readRobotInfo(const std::string& robotFile)
@@ -466,6 +467,18 @@ namespace armarx
(void) jointStatuses, (void) timestamp, (void) aValueChanged;
}
+ void RobotStateComponent::simulatorWasReset(const Current&)
+ {
+ {
+ std::lock_guard lock(poseHistoryMutex);
+ poseHistory.clear();
+ }
+ {
+ std::lock_guard lock(jointHistoryMutex);
+ jointHistory.clear();
+ }
+ }
+
PropertyDefinitionsPtr RobotStateComponent::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new RobotStatePropertyDefinitions(
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index 2a1da853b..517ed1a84 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -179,7 +179,7 @@ namespace armarx
/// Does nothing.
void reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
-
+ void simulatorWasReset(const Ice::Current& = Ice::emptyCurrent) override;
private:
void readRobotInfo(const std::string& robotFile);
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index 44b72a66c..332959df8 100644
--- a/source/RobotAPI/interface/core/RobotState.ice
+++ b/source/RobotAPI/interface/core/RobotState.ice
@@ -23,6 +23,7 @@
*/
#pragma once
+#include <ArmarXCore/interface/events/SimulatorResetEvent.ice>
#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
#include <RobotAPI/interface/core/FramedPoseBase.ice>
@@ -191,7 +192,8 @@ module armarx
interface RobotStateComponentInterface extends
KinematicUnitListener,
PlatformUnitListener,
- GlobalRobotPoseLocalizationListener
+ GlobalRobotPoseLocalizationListener,
+ SimulatorResetEvent
{
/**
* @return proxy to the shared robot which constantly updates all joint values
--
GitLab