YARP
Yet Another Robot Platform
FrameTransformContainer.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: LGPL-2.1-or-later
4 */
5
7
8#include <yarp/os/Log.h>
10#include <yarp/os/LogStream.h>
11#include <algorithm>
12
13using namespace yarp::dev;
14using namespace yarp::os;
15using namespace yarp::sig;
16using namespace yarp::math;
17
18namespace {
19YARP_LOG_COMPONENT(FRAMETRANSFORMCONTAINER, "yarp.device.frameTransformContainer")
20
21}
22
23//------------------------------------------------------------------------------------------------------------------------------
24
26{
28 trf.isStatic = false;
29 trf.translation = { 0,0,0 };
30 trf.rotation = { 0,0,0,0 };
32 {
33 yCIDebug(FRAMETRANSFORMCONTAINER, m_name) << "At time" << std::to_string(trf.timestamp)
34 << "deleted transform (marked invalid):" << trf.src_frame_id << "->" << trf.dst_frame_id << ", assigned timestamp" << std::to_string(trf.timestamp);
35 }
36}
37
38bool FrameTransformContainer::getTransforms(std::vector<yarp::math::FrameTransform>& transforms) const
39{
40 std::lock_guard<std::recursive_mutex> lock(m_trf_mutex);
41 std::copy_if (m_transforms.begin(), m_transforms.end(), std::back_inserter(transforms), [](const yarp::math::FrameTransform& trf){return trf.isValid();});
42 return true;
43}
44
45bool FrameTransformContainer::setTransforms(const std::vector<yarp::math::FrameTransform>& transforms)
46{
47 for (auto& it : transforms)
48 {
49 setTransform(it);
50 }
51 return true;
52}
53
55{
56 if (new_tr.isValid()==false) return true;
57
58 std::lock_guard<std::recursive_mutex> lock(m_trf_mutex);
59 for (auto& it : m_transforms)
60 {
61 //this linear search may require some optimization
62 if (it.dst_frame_id == new_tr.dst_frame_id && it.src_frame_id == new_tr.src_frame_id)
63 {
64 //if transform already exists and
65 //its timestamp is more recent than the currently stored transform
66 //than update it
67 if (it.isStatic == false)
68 {
69 if (new_tr.timestamp > it.timestamp)
70 {
71 it = new_tr;
72 return true;
73 }
74 else
75 {
76 //yCDebug(FRAMETRANSFORMCONTAINER) << "Received old transform" << it.dst_frame_id << it.src_frame_id << std::to_string(it.timestamp) << it.isStatic;
77 return true;
78 }
79 }
80 else
81 {
82 return true;
83 }
84 }
85 }
86
87 //add a new transform
88 m_transforms.push_back(new_tr);
89 return true;
90}
91
92bool FrameTransformContainer::deleteTransform(std::string t1, std::string t2)
93{
94 std::lock_guard<std::recursive_mutex> lock(m_trf_mutex);
95 if (t1 == "*" && t2 == "*")
96 {
97 for (size_t i = 0; i < m_transforms.size(); i++)
98 {
100 }
101 return true;
102 }
103 else
104 {
105 if (t1 == "*")
106 {
107 for (size_t i = 0; i < m_transforms.size(); i++)
108 {
109 //source frame is jolly, thus delete all frames with destination == t2
110 if (m_transforms[i].dst_frame_id == t2)
111 {
113 }
114 }
115 return true;
116 }
117 else
118 {
119 if (t2 == "*")
120 {
121 for (size_t i = 0; i < m_transforms.size(); i++)
122 {
123 //destination frame is jolly, thus delete all frames with source == t1
124 if (m_transforms[i].src_frame_id == t1)
125 {
127 }
128 }
129 return true;
130 }
131 else
132 {
133 for (size_t i = 0; i < m_transforms.size(); i++)
134 {
135 if ((m_transforms[i].dst_frame_id == t1 && m_transforms[i].src_frame_id == t2) ||
136 (m_transforms[i].dst_frame_id == t2 && m_transforms[i].src_frame_id == t1))
137 {
139 return true;
140 }
141 }
142 }
143 }
144 }
145
146 yCError(FRAMETRANSFORMCONTAINER) << "Transformation deletion not successful";
147 return false;
148}
149
151{
152 std::lock_guard<std::recursive_mutex> lock(m_trf_mutex);
153 for (size_t i = 0; i < m_transforms.size(); i++)
154 {
156 }
157 return true;
158}
159
161{
162 std::lock_guard<std::recursive_mutex> lock(m_trf_mutex);
163 double curr_t = yarp::os::Time::now();
164 check_vector:
165 for (auto it= m_transforms.begin(); it!= m_transforms.end(); it++)
166 {
167 if (curr_t - it->timestamp > m_timeout &&
168 it->isStatic == false)
169 {
170 if (m_verbose_debug)
171 {
172 if (it->isValid())
173 {yCIDebug(FRAMETRANSFORMCONTAINER, m_name) << "At time" << std::to_string(curr_t)
174 <<"Transform expired:" << it->src_frame_id << "->" << it->dst_frame_id << "with timestamp" << std::to_string(it->timestamp);}
175 else
176 {yCIDebug(FRAMETRANSFORMCONTAINER, m_name) << "At time" << std::to_string(curr_t)
177 << "Invalid transform expired:" << it->src_frame_id << "->"<< it->dst_frame_id << "with timestamp" << std::to_string(it->timestamp);}
178 }
179 m_transforms.erase(it);
180 goto check_vector;
181 }
182 }
183 return true;
184}
185
187{
188 return const_cast<FrameTransformContainer*>(this)->checkAndRemoveExpired();
189}
190
191bool FrameTransformContainer::size(size_t& size) const
192{
193 std::lock_guard<std::recursive_mutex> lock(m_trf_mutex);
194 size = m_transforms.size();
195 return true;
196}
FrameTransformContainer: A class that contains a vector of frame transformations and exposes yarp::de...
void invalidateTransform(yarp::math::FrameTransform &trf)
bool setTransform(const yarp::math::FrameTransform &transform) override
Save a frame transform in a storage.
bool getTransforms(std::vector< yarp::math::FrameTransform > &transforms) const override
Obtains all frame transforms saved in a storage.
bool size(size_t &size) const
bool setTransforms(const std::vector< yarp::math::FrameTransform > &transforms) override
Save some frame transforms in a storage.
bool deleteTransform(std::string t1, std::string t2) override
Delete a single transform in the storage.
std::recursive_mutex m_trf_mutex
bool clearAll() override
Delete all transforms in a storage.
struct yarp::math::FrameTransform::Translation_t translation
#define yCError(component,...)
Definition: LogComponent.h:213
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:76
#define yCIDebug(component, id,...)
Definition: LogComponent.h:138
std::string to_string(IntegerType x)
Definition: numeric.h:115
For streams capable of holding different kinds of content, check what they actually have.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition: Time.cpp:121
An interface to the operating system, including Port based communication.