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>
9 #include <yarp/os/LogComponent.h>
10 #include <yarp/os/LogStream.h>
11 #include <algorithm>
12 
13 using namespace yarp::dev;
14 using namespace yarp::os;
15 using namespace yarp::sig;
16 using namespace yarp::math;
17 
18 namespace {
19 YARP_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 };
31  if (m_verbose_debug)
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 
38 bool 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 
45 bool 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 
92 bool 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  {
99  invalidateTransform(m_transforms[i]);
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  {
112  invalidateTransform(m_transforms[i]);
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  {
126  invalidateTransform(m_transforms[i]);
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  {
138  invalidateTransform(m_transforms[i]);
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  {
155  invalidateTransform(m_transforms[i]);
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 
191 bool 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.
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.