diff --git a/src/joint_state_listener.cpp b/src/joint_state_listener.cpp index c89fede..84e562b 100644 --- a/src/joint_state_listener.cpp +++ b/src/joint_state_listener.cpp @@ -36,7 +36,9 @@ #include #include +#include #include +#include #include #include @@ -48,6 +50,37 @@ using namespace robot_state_publisher; +namespace robot_state_publisher { + +template +class ParameterCache { +public: + void set(const void* owner, const T& value) + { + std::lock_guard lock(mtx_); + store_[owner] = value; + } + bool get(const void* owner, T& value) + { + std::lock_guard lock(mtx_); + if (store_.count(owner) > 0) { + value = store_.at(owner); + return true; + } + return false; + } + void clear(const void* owner) + { + std::lock_guard lock(mtx_); + store_.erase(owner); + } +private: + std::mutex mtx_; // shared_mutex is preferable + std::unordered_map store_; +}; +ParameterCache g_tf_prefix_cache; +} + JointStateListener::JointStateListener() : JointStateListener(KDL::Tree(), MimicMap()) { } @@ -88,17 +121,24 @@ JointStateListener::JointStateListener(const std::shared_ptr