YARP
Yet Another Robot Platform
FrameTransformClient.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 
6 #define _USE_MATH_DEFINES
7 #include <cmath>
8 
9 #include "FrameTransformClient.h"
10 
11 #include <yarp/os/Log.h>
12 #include <yarp/os/LogComponent.h>
13 #include <yarp/os/LogStream.h>
14 #include <yarp/math/Math.h>
15 
16 #include <cmrc/cmrc.hpp>
17 #include <mutex>
18 #include <fstream>
19 
20 CMRC_DECLARE(frameTransformRC);
21 
22 using namespace yarp::dev;
23 using namespace yarp::os;
24 using namespace yarp::sig;
25 using namespace yarp::math;
26 
27 namespace {
28 YARP_LOG_COMPONENT(FRAMETRANSFORMCLIENT, "yarp.device.FrameTransformClient")
29 }
30 #define LOG_THROTTLE_PERIOD 1.0
31 
32 //------------------------------------------------------------------------------------------------------------------------------
34 {
35  std::lock_guard<std::mutex> lock (m_rpc_mutex);
37  yarp::os::Bottle out;
38  bool ok = in.read(connection);
39  if (!ok) {
40  return false;
41  }
42 
43  std::string request = in.get(0).asString();
44  if (request == "help")
45  {
46  out.addVocab32("many");
47  out.addString("get_transform <src> <dst>: print the transform from <src> to <dst>");
48  out.addString("list_transforms: print all the stored frame transforms");
49  out.addString("list_frames: print all the available reference frames");
50  out.addString("list_ports: print all the opened ports for transform broadcasting");
51  out.addString("publish_transform <src> <dst> <portname> [format]: opens a port to publish transform from src to dst");
52  out.addString("unpublish_transform <portname>: closes a previously opened port to publish a transform");
53  out.addString("unpublish_all: closes a all previously opened ports to publish a transform");
54  out.addString("set_static_transform_rad <src> <dst> <x> <y> <z> <roll> <pitch> <yaw>': create a static transform (angles in radians)");
55  out.addString("set_static_transform_deg <src> <dst> <x> <y> <z> <roll> <pitch> <yaw>': create a static transform (angles in degrees)");
56  out.addString("delete_static_transform <src> <dst>': delete a static transform");
57  out.addString("generate_view <option>': generate a frames.pdf file showing the transform tree diagram.");
58  out.addString(" The following values are valid for option (default=none)");
59  out.addString(" 'show_rpy': show rotation as rpy angles");
60  out.addString(" 'show_quaterion:'show rotation as a quaternion");
61  out.addString(" 'show_matrix:'show rotation as a 3x3 rotation matrix");
62  }
63  else if (request == "list_frames")
64  {
65  std::vector<std::string> v;
66  this->getAllFrameIds(v);
67  out.addVocab32("many");
68  out.addString("List of available reference frames:");
69  int count = 0;
70  for (auto& vec : v)
71  {
72  count++;
73  std::string str = std::to_string(count) + "- " + vec;
74  out.addString(str.c_str());
75  }
76  }
77  else if (request == "get_transform")
78  {
79  std::string src = in.get(1).asString();
80  std::string dst = in.get(2).asString();
81  out.addVocab32("many");
83  this->getTransform(src, dst, m);
84  out.addString("Transform from " + src + " to " + dst + " is: ");
85  out.addString(m.toString());
86  }
87  else if (request == "list_ports")
88  {
89  out.addVocab32("many");
90  if (m_array_of_ports.size()==0)
91  {
92  out.addString("No ports are currently active");
93  }
94  for (auto& m_array_of_port : m_array_of_ports)
95  {
96  if (m_array_of_port)
97  {
98  std::string s = m_array_of_port->port.getName() + " "+ m_array_of_port->transform_src + " -> " + m_array_of_port->transform_dst;
99  out.addString(s);
100  }
101  }
102  }
103  else if (request == "publish_transform")
104  {
105  out.addVocab32("many");
106  std::string src = in.get(1).asString();
107  std::string dst = in.get(2).asString();
108  std::string port_name = in.get(3).asString();
109  std::string format_s = "matrix";
110  broadcast_port_t::format_t eformat = broadcast_port_t::format_t::matrix;
111  if (in.size() == 5) {format_s = in.get(4).asString();}
112 
113  if (format_s == "matrix") {eformat = broadcast_port_t::format_t::matrix;}
114  else {yCError(FRAMETRANSFORMCLIENT) << "Invalid format" << format_s << "using format `matrix`. Only `matrix` is currently supported."; }
115 
116  if (port_name[0] == '/') {
117  port_name.erase(port_name.begin());
118  }
119  std::string full_port_name = m_local_name + "/" + port_name;
120 
121  //print a warning if the frames do not exists yet
122  if (this->frameExists(src) == false)
123  {
124  out.addString("Requested src frame " + src + " does not exists.");
125  yCWarning(FRAMETRANSFORMCLIENT, "Requested src frame %s does not exists.", src.c_str());
126  }
127  if (this->frameExists(dst) == false)
128  {
129  out.addString("Requested dst frame " + dst + " does not exists.");
130  yCWarning(FRAMETRANSFORMCLIENT, "Requested fst frame %s does not exists.", dst.c_str());
131  }
132 
133  //check if the port is already active in yarp
134  bool port_already_exists = yarp::os::Network::exists(full_port_name);
135  if (port_already_exists) { yCError(FRAMETRANSFORMCLIENT, "Requested port already exists!"); }
136 
137  //check if the port is already registered by the system
138  bool bcast_already_exists = false;
139  for (auto& m_array_of_port : m_array_of_ports)
140  {
141  if (m_array_of_port && m_array_of_port->port.getName() == full_port_name)
142  {
143  bcast_already_exists |= true;
144  break;
145  }
146  }
147 
148  //create the broadcaster
149  if (bcast_already_exists == false && port_already_exists == false)
150  {
151  auto* b = new broadcast_port_t;
152  b->transform_src = src;
153  b->transform_dst = dst;
154  b->format = eformat;
155  bool pret = b->port.open(full_port_name);
156  if (pret)
157  {
158  out.addString("Operation complete. Port " + full_port_name + " opened.");
159  m_array_of_ports.push_back(b);
160  if (m_array_of_ports.size() == 1) {
161  this->start();
162  }
163  }
164  else
165  {
166  delete b;
167  out.addString("Operation failed. Unable to open port " + full_port_name + ".");
168  }
169  }
170  else
171  {
172  out.addString("unable to perform operation");
173  }
174  }
175  else if (request == "unpublish_all")
176  {
177  for (auto& m_array_of_port : m_array_of_ports)
178  {
179  m_array_of_port->port.close();
180  delete m_array_of_port;
181  m_array_of_port=nullptr;
182  }
183  m_array_of_ports.clear();
184  if (m_array_of_ports.size() == 0) {
185  this->askToStop();
186  }
187  out.addString("Operation complete");
188  }
189  else if (request == "unpublish_transform")
190  {
191  bool ret = false;
192  std::string port_name = in.get(1).asString();
193  if (port_name[0] == '/') {
194  port_name.erase(port_name.begin());
195  }
196  std::string full_port_name = m_local_name + "/" + port_name;
197  for (auto it = m_array_of_ports.begin(); it != m_array_of_ports.end(); it++)
198  {
199  if ((*it)->port.getName() == port_name)
200  {
201  (*it)->port.close();
202  delete (*it);
203  (*it)=nullptr;
204  m_array_of_ports.erase(it);
205  ret = true;
206  break;
207  }
208  }
209  if (ret)
210  {
211  out.addString("Port " + full_port_name + " has been closed.");
212  }
213  else
214  {
215  out.addString("Port " + full_port_name + " was not found.");
216  }
217  if (m_array_of_ports.size() == 0) {
218  this->askToStop();
219  }
220  }
221  else if (request == "set_static_transform_rad" ||
222  request == "set_static_transform_deg")
223  {
225  t.src_frame_id = in.get(1).asString();
226  t.dst_frame_id = in.get(2).asString();
227  t.translation.tX = in.get(3).asFloat64();
228  t.translation.tY = in.get(4).asFloat64();
229  t.translation.tZ = in.get(5).asFloat64();
230  if (request == "set_static_transform_rad")
231  {
232  t.rotFromRPY(in.get(6).asFloat64(),
233  in.get(7).asFloat64(),
234  in.get(8).asFloat64());
235  }
236  else if (request == "set_static_transform_deg")
237  {
238  t.rotFromRPY(in.get(6).asFloat64() * 180 / M_PI,
239  in.get(7).asFloat64() * 180 / M_PI,
240  in.get(8).asFloat64() * 180 / M_PI);
241  }
242  t.timestamp = yarp::os::Time::now();
243  bool ret = this->setTransformStatic(t.src_frame_id,t.dst_frame_id,t.toMatrix());
244  if (ret == true)
245  {
246  yCInfo(FRAMETRANSFORMCLIENT) << "set_static_transform done";
247  out.addString("set_static_transform done");
248  }
249  else
250  {
251  yCError(FRAMETRANSFORMCLIENT) << "read(): something strange happened";
252  out.addString("error");
253  }
254  }
255  else if (request == "generate_view")
256  {
257  m_show_transforms_in_diagram = show_transforms_in_diagram_t::do_not_show;
258  if (in.get(1).asString() == "show_quaternion") {
259  m_show_transforms_in_diagram = show_transforms_in_diagram_t::show_quaternion;
260  } else if (in.get(1).asString() == "show_matrix") {
261  m_show_transforms_in_diagram = show_transforms_in_diagram_t::show_matrix;
262  } else if (in.get(1).asString() == "show_rpy") {
263  m_show_transforms_in_diagram = show_transforms_in_diagram_t::show_rpy;
264  }
265  priv_generate_view();
266  out.addString("ok");
267  }
268  else if (request == "delete_static_transform")
269  {
270  std::string src = in.get(1).asString();
271  std::string dst = in.get(2).asString();
272  //@@@check if the transform is static?
273  this->deleteTransform(src,dst);
274  out.addString("delete_static_transform done");
275  }
276  else
277  {
278  yCError(FRAMETRANSFORMCLIENT, "Invalid vocab received in FrameTransformClient");
279  out.clear();
280  out.addVocab32(VOCAB_ERR);
281  out.addString("Invalid command name");
282  }
283 
284  yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
285  if (returnToSender != nullptr)
286  {
287  out.write(*returnToSender);
288  }
289  else
290  {
291  yCError(FRAMETRANSFORMCLIENT) << "Invalid return to sender";
292  }
293  return true;
294 }
295 
297 {
298  yCWarning(FRAMETRANSFORMCLIENT) << "The 'FrameTransformClient' device is experimental and could be modified without any warning";
299 
300  std::string cfg_string = config.toString();
301  yarp::os::Property cfg;
302  cfg.fromString(cfg_string);
303 
304  std::string configuration_to_open;
305  std::string innerFilePath="config_xml/ftc_local_only.xml";
306  if(cfg.check("testxml_option"))
307  {
308  innerFilePath = cfg.find("testxml_option").asString();
309  std::ifstream xmlFile(innerFilePath);
310  std::stringstream stream_file;
311  stream_file << xmlFile.rdbuf();
312  configuration_to_open = stream_file.str();
313  }
314  else
315  {
316  auto fs = cmrc::frameTransformRC::get_filesystem();
317  if(cfg.check("filexml_option")) { innerFilePath="config_xml/"+cfg.find("filexml_option").toString();}
318  cfg.unput("filexml_option");
319  auto xmlFile = fs.open(innerFilePath);
320  for(const auto& lemma : xmlFile)
321  {
322  configuration_to_open += lemma;
323  }
324  }
325 
326  std::string m_local_rpcUser = "/ftClient/rpc";
327 
328  if (cfg.check("local_rpc")) { m_local_rpcUser=cfg.find("local_rpc").toString();}
329  cfg.unput("local_rpc");
330 
331  if (config.check("FrameTransform_verbose_debug"))
332  {
333  yarp::os::Value vval = config.find("FrameTransform_verbose_debug");
334  cfg.put("FrameTransform_verbose_debug", vval);
335  }
336 
338  yarp::robotinterface::XMLReaderResult result = reader.getRobotFromString(configuration_to_open, cfg);
339  yCAssert(FRAMETRANSFORMCLIENT, result.parsingIsSuccessful);
340 
341  m_robot = std::move(result.robot);
342 
343  if (!m_robot.enterPhase(yarp::robotinterface::ActionPhaseStartup)) {
344  return false;
345  }
346 
347  if (!m_robot.enterPhase(yarp::robotinterface::ActionPhaseRun)) {
348  return false;
349  }
350 
351  std::string setdeviceName = "ftc_storage";
352  if (m_robot.hasParam("setDeviceName")) { setdeviceName = m_robot.findParam("setDeviceName");}
353  yCAssert(FRAMETRANSFORMCLIENT, m_robot.hasDevice(setdeviceName));
354  auto* polyset = m_robot.device(setdeviceName).driver();
355  yCAssert(FRAMETRANSFORMCLIENT, polyset);
356  polyset->view(m_ift_set);
357  yCAssert(FRAMETRANSFORMCLIENT, m_ift_set);
358 
359  std::string getdeviceName = "ftc_storage";
360  if (m_robot.hasParam("getDeviceName")) {getdeviceName = m_robot.findParam("getDeviceName");}
361  yCAssert(FRAMETRANSFORMCLIENT, m_robot.hasDevice(getdeviceName));
362  auto* polyget = m_robot.device(getdeviceName).driver();
363  yCAssert(FRAMETRANSFORMCLIENT, polyget);
364  polyget->view(m_ift_get);
365  yCAssert(FRAMETRANSFORMCLIENT, m_ift_get);
366  polyget->view(m_ift_util);
367  yCAssert(FRAMETRANSFORMCLIENT, m_ift_util);
368 
369  if (config.check("period"))
370  {
371  m_period = config.find("period").asFloat64();
372  this->setPeriod(m_period);
373  }
374 
375  if (!m_rpc_InterfaceToUser.open(m_local_rpcUser))
376  {
377  yCError(FRAMETRANSFORMCLIENT,"Failed to open rpc port");
378  }
379 
380  m_rpc_InterfaceToUser.setReader(*this);
381 
382  return true;
383 }
384 
386 {
387  this->askToStop();
388  m_rpc_InterfaceToUser.close();
389  yCDebug(FRAMETRANSFORMCLIENT, "rpc port closed");
390 
392  m_robot.enterPhase(yarp::robotinterface::ActionPhaseShutdown);
393 
394  return true;
395 }
396 
397 bool FrameTransformClient::allFramesAsString(std::string &all_frames)
398 {
399  FrameTransformContainer* p_cont = nullptr;
400  bool br = m_ift_util->getInternalContainer(p_cont);
401  if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return false; }
402 
403  //protect the internal container from concurrent access
404  std::lock_guard<std::recursive_mutex> l(p_cont->m_trf_mutex);
405 
406  //process data
407  for (auto it = p_cont->begin(); it != p_cont->end(); it++)
408  {
409  all_frames += it->toString() + " ";
410  }
411  return true;
412 }
413 
414 FrameTransformClient::ConnectionType FrameTransformClient::priv_getConnectionType(const std::string &target_frame, const std::string &source_frame, std::string* commonAncestor = nullptr)
415 {
416  if (target_frame == source_frame) {return ConnectionType::IDENTITY;}
417 
418  FrameTransformContainer* p_cont = nullptr;
419  bool br = m_ift_util->getInternalContainer(p_cont);
420  if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return ConnectionType::DISCONNECTED; }
421 
422  size_t i, j;
423  std::vector<std::string> tar2root_vec;
424  std::vector<std::string> src2root_vec;
425  std::string ancestor, child;
426  child = target_frame;
427 
428  //protect the internal container from concurrent access
429  std::lock_guard<std::recursive_mutex> l(p_cont->m_trf_mutex);
430 
431  //process data
432  while(getParent(child, ancestor))
433  {
434  if(ancestor == source_frame)
435  {
436  return ConnectionType::DIRECT;
437  }
438 
439  tar2root_vec.push_back(ancestor);
440  child = ancestor;
441  }
442  child = source_frame;
443  while(getParent(child, ancestor))
444  {
445  if(ancestor == target_frame)
446  {
447  return ConnectionType::INVERSE;
448  }
449 
450  src2root_vec.push_back(ancestor);
451  child = ancestor;
452  }
453 
454  for(i = 0; i < tar2root_vec.size(); i++)
455  {
456  std::string a;
457  a = tar2root_vec[i];
458  for(j = 0; j < src2root_vec.size(); j++)
459  {
460  if(a == src2root_vec[j])
461  {
462  if(commonAncestor)
463  {
464  *commonAncestor = a;
465  }
466  return ConnectionType::UNDIRECT;
467  }
468  }
469  }
470 
471  return ConnectionType::DISCONNECTED;
472 }
473 
474 bool FrameTransformClient::canTransform(const std::string &target_frame, const std::string &source_frame)
475 {
476  return priv_getConnectionType(target_frame, source_frame) != ConnectionType::DISCONNECTED;
477 }
478 
480 {
481  if (m_ift_set)
482  {
483  return m_ift_set->clearAll();
484  }
485  yCError(FRAMETRANSFORMCLIENT, "clear(): interface not available");
486  return false;
487 }
488 
489 bool FrameTransformClient::frameExists(const std::string &frame_id)
490 {
491  FrameTransformContainer* p_cont = nullptr;
492  bool br = m_ift_util->getInternalContainer(p_cont);
493  if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return false; }
494 
495  //protect the internal container from concurrent access
496  std::lock_guard<std::recursive_mutex> l(p_cont->m_trf_mutex);
497 
498  //process data
499  for (auto it = p_cont->begin(); it != p_cont->end(); it++)
500  {
501  if (it->src_frame_id == frame_id) { return true; }
502  if (it->dst_frame_id == frame_id) { return true; }
503  }
504 
505  // condition reached if not found or container is empty
506  return false;
507 }
508 
509 bool FrameTransformClient::getAllFrameIds(std::vector<std::string> &ids)
510 {
511  FrameTransformContainer* p_cont = nullptr;
512  bool br = m_ift_util->getInternalContainer(p_cont);
513  if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return false; }
514 
515  //protect the internal container from concurrent access
516  std::lock_guard<std::recursive_mutex> l(p_cont->m_trf_mutex);
517 
518  //process data
519  for (auto it = p_cont->begin(); it != p_cont->end(); it++)
520  {
521  bool found = false;
522  for (const auto& id : ids)
523  {
524  if (it->src_frame_id == id) { found = true; break; }
525  }
526  if (found == false) {
527  ids.push_back(it->src_frame_id);
528  }
529  }
530 
531  for (auto it = p_cont->begin(); it != p_cont->end(); it++)
532  {
533  bool found = false;
534  for (const auto& id : ids)
535  {
536  if (it->dst_frame_id == id) { found = true; break; }
537  }
538  if (found == false) {
539  ids.push_back(it->dst_frame_id);
540  }
541  }
542 
543  return true;
544 }
545 
546 bool FrameTransformClient::getParent(const std::string &frame_id, std::string &parent_frame_id)
547 {
548  FrameTransformContainer* p_cont = nullptr;
549  bool br = m_ift_util->getInternalContainer(p_cont);
550  if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return false; }
551 
552  //protect the internal container from concurrent access
553  std::lock_guard<std::recursive_mutex> l(p_cont->m_trf_mutex);
554 
555  //process data
556  for (auto it = p_cont->begin(); it != p_cont->end(); it++)
557  {
558  std::string par(it->dst_frame_id);
559  if (it->dst_frame_id == frame_id)
560  {
561  parent_frame_id = it->src_frame_id;
562  return true;
563  }
564  }
565  return false;
566 }
567 
568 bool FrameTransformClient::priv_canExplicitTransform(const std::string& target_frame_id, const std::string& source_frame_id) const
569 {
570  FrameTransformContainer* p_cont = nullptr;
571  bool br = m_ift_util->getInternalContainer(p_cont);
572  if (!br || p_cont == nullptr) {yCError(FRAMETRANSFORMCLIENT) << "Failure"; return false; }
573 
574  //protect the internal container from concurrent access
575  std::lock_guard<std::recursive_mutex> l(p_cont->m_trf_mutex);
576 
577  //process data
578  for (auto it = p_cont->begin(); it != p_cont->end(); it++)
579  {
580  if (it->dst_frame_id == target_frame_id && it->src_frame_id == source_frame_id)
581  {
582  return true;
583  }
584  }
585  return false;
586 }
587 
588 bool FrameTransformClient::priv_getChainedTransform(const std::string& target_frame_id, const std::string& source_frame_id, yarp::sig::Matrix& transform) const
589 {
590  FrameTransformContainer* p_cont = nullptr;
591  bool br = m_ift_util->getInternalContainer(p_cont);
592  if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return false; }
593 
594  //protect the internal container from concurrent access
595  std::lock_guard<std::recursive_mutex> l(p_cont->m_trf_mutex);
596 
597  //process data
598  for (auto it = p_cont->begin(); it != p_cont->end(); it++)
599  {
600  if (it->dst_frame_id == target_frame_id)
601  {
602  if (it->src_frame_id == source_frame_id)
603  {
604  transform = it->toMatrix();
605  return true;
606  }
607  else
608  {
610  if (priv_getChainedTransform(it->src_frame_id, source_frame_id, m))
611  {
612  transform = m * it->toMatrix();
613  return true;
614  }
615  }
616  }
617  }
618  return false;
619 }
620 
621 bool FrameTransformClient::getTransform(const std::string& target_frame_id, const std::string& source_frame_id, yarp::sig::Matrix& transform)
622 {
623  ConnectionType ct;
624  std::string ancestor;
625  ct = priv_getConnectionType(target_frame_id, source_frame_id, &ancestor);
626  if (ct == ConnectionType::DIRECT)
627  {
628  return priv_getChainedTransform(target_frame_id, source_frame_id, transform);
629  }
630  else if (ct == ConnectionType::INVERSE)
631  {
632  yarp::sig::Matrix m(4, 4);
633  priv_getChainedTransform(source_frame_id, target_frame_id, m);
634  transform = yarp::math::SE3inv(m);
635  return true;
636  }
637  else if(ct == ConnectionType::UNDIRECT)
638  {
639  yarp::sig::Matrix root2tar(4, 4), root2src(4, 4);
640  priv_getChainedTransform(source_frame_id, ancestor, root2src);
641  priv_getChainedTransform(target_frame_id, ancestor, root2tar);
642  transform = yarp::math::SE3inv(root2src) * root2tar;
643  return true;
644  }
645  else if (ct == ConnectionType::IDENTITY)
646  {
647  yarp::sig::Matrix tmp(4, 4); tmp.eye();
648  transform = tmp;
649  return true;
650  }
651 
652  yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "getTransform(): Frames " << source_frame_id << " and " << target_frame_id << " are not connected";
653  return false;
654 }
655 
656 bool FrameTransformClient::setTransform(const std::string& target_frame_id, const std::string& source_frame_id, const yarp::sig::Matrix& transform)
657 {
658  if(target_frame_id == source_frame_id)
659  {
660  yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "setTransform(): Invalid transform detected.\n" \
661  "\t Source frame and target frame are both equal to " << source_frame_id;
662  return false;
663  }
664 
665  if (!priv_canExplicitTransform(target_frame_id, source_frame_id) && canTransform(target_frame_id, source_frame_id))
666  {
667  yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "setTransform(): Such transform already exist by chaining transforms";
668  return false;
669  }
670 
672  yarp::os::Bottle resp;
674 
675  if (!tf.fromMatrix(transform))
676  {
677  yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "setTransform(): Wrong matrix format, it has to be 4 by 4";
678  return false;
679  }
680 
681  tf.src_frame_id = source_frame_id;
682  tf.dst_frame_id = target_frame_id;
683  tf.isStatic = false;
684  tf.timestamp = yarp::os::Time::now();
685 
686  if (m_ift_set)
687  {
688  return m_ift_set->setTransform(tf);
689  }
690  yCError(FRAMETRANSFORMCLIENT, "setTransform(): interface not available");
691  return false;
692 }
693 
694 bool FrameTransformClient::setTransformStatic(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform)
695 {
696  if(target_frame_id == source_frame_id)
697  {
698  yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "setTransformStatic(): Invalid transform detected.\n" \
699  "\t Source frame and target frame are both equal to " << source_frame_id;
700  return false;
701  }
702 
703  if (canTransform(target_frame_id, source_frame_id))
704  {
705  yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "setTransform(): Such static transform already exist, directly or by chaining transforms";
706  return false;
707  }
708 
710  if (!tf.fromMatrix(transform))
711  {
712  yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "setTransform(): Wrong matrix format, it has to be 4 by 4";
713  return false;
714  }
715  tf.src_frame_id = source_frame_id;
716  tf.dst_frame_id = target_frame_id;
717  tf.isStatic = true;
718  tf.timestamp= yarp::os::Time::now();
719 
720  if (m_ift_set)
721  {
722  return m_ift_set->setTransform(tf);
723  }
724  yCError(FRAMETRANSFORMCLIENT, "setTransformStatic(): interface not available");
725  return false;
726 }
727 
728 bool FrameTransformClient::deleteTransform(const std::string &target_frame_id, const std::string &source_frame_id)
729 {
730  if (m_ift_set)
731  {
732  return m_ift_set->deleteTransform(target_frame_id, source_frame_id);
733  }
734  yCError(FRAMETRANSFORMCLIENT, "deleteTransform(): interface not available");
735  return false;
736 }
737 
738 bool FrameTransformClient::transformPoint(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_point, yarp::sig::Vector &transformed_point)
739 {
740  if (input_point.size() != 3)
741  {
742  yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "Only 3 dimensional vector allowed.";
743  return false;
744  }
745  yarp::sig::Matrix m(4, 4);
746  if (!getTransform(target_frame_id, source_frame_id, m))
747  {
748  yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "No transform found between source '" << target_frame_id << "' and target '" << source_frame_id << "'";
749  return false;
750  }
751  yarp::sig::Vector in = input_point;
752  in.push_back(1);
753  transformed_point = m * in;
754  transformed_point.pop_back();
755  return true;
756 }
757 
758 bool FrameTransformClient::transformPose(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_pose, yarp::sig::Vector &transformed_pose)
759 {
760  if (input_pose.size() != 6)
761  {
762  yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "Only 6 dimensional vector (3 axes + roll pith and yaw) allowed.";
763  return false;
764  }
765  if (transformed_pose.size() != 6)
766  {
767  yCWarning(FRAMETRANSFORMCLIENT, "transformPose(): Performance warning: size transformed_pose should be 6, resizing.");
768  transformed_pose.resize(6, 0.0);
769  }
770  yarp::sig::Matrix m(4, 4);
771  if (!getTransform(target_frame_id, source_frame_id, m))
772  {
773  yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "No transform found between source '" << target_frame_id << "' and target '" << source_frame_id << "'";
774  return false;
775  }
777  t.transFromVec(input_pose[0], input_pose[1], input_pose[2]);
778  t.rotFromRPY(input_pose[3], input_pose[4], input_pose[5]);
779  t.fromMatrix(m * t.toMatrix());
780  transformed_pose[0] = t.translation.tX;
781  transformed_pose[1] = t.translation.tY;
782  transformed_pose[2] = t.translation.tZ;
783 
784  yarp::sig::Vector rot;
785  rot = t.getRPYRot();
786  transformed_pose[3] = rot[0];
787  transformed_pose[4] = rot[1];
788  transformed_pose[5] = rot[2];
789  return true;
790 }
791 
792 bool FrameTransformClient::transformQuaternion(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::math::Quaternion &input_quaternion, yarp::math::Quaternion &transformed_quaternion)
793 {
794  yarp::sig::Matrix m(4, 4);
795  if (!getTransform(target_frame_id, source_frame_id, m))
796  {
797  yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "No transform found between source '" << target_frame_id << "' and target '" << source_frame_id <<"'";
798  return false;
799  }
801  t.rotation=input_quaternion;
802  transformed_quaternion.fromRotationMatrix(m * t.toMatrix());
803  return true;
804 }
805 
806 bool FrameTransformClient::waitForTransform(const std::string &target_frame_id, const std::string &source_frame_id, const double &timeout)
807 {
808  //loop until canTransform == true or timeout expires
809  double start = yarp::os::SystemClock::nowSystem();
810  while (!canTransform(target_frame_id, source_frame_id))
811  {
812  if (yarp::os::SystemClock::nowSystem() - start > timeout)
813  {
814  yCError(FRAMETRANSFORMCLIENT) << "waitForTransform(): timeout expired";
815  return false;
816  }
818  }
819  return true;
820 }
821 
823  m_period(0.01)
824 {
825 }
826 
828 
830 {
831  yCTrace(FRAMETRANSFORMCLIENT, "Thread started");
832  return true;
833 }
834 
836 {
837  yCTrace(FRAMETRANSFORMCLIENT, "Thread stopped");
838 }
839 
841 {
842  std::lock_guard<std::mutex> lock (m_rpc_mutex);
843  if (m_array_of_ports.size()==0)
844  {
845  return;
846  }
847 
848  for (auto& m_array_of_port : m_array_of_ports)
849  {
850  if (m_array_of_port)
851  {
852  std::string src = m_array_of_port->transform_src;
853  std::string dst = m_array_of_port->transform_dst;
855  this->getTransform(src, dst, m);
856  if (m_array_of_port->format == broadcast_port_t::format_t::matrix)
857  {
858  m_array_of_port->port.write(m);
859  }
860  else
861  {
862  yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "Unknown format requested: " << (int)(m_array_of_port->format);
863  }
864  }
865  }
866 }
867 
869 {
870  if (t == nullptr) {
871  return "";
872  }
873 
875  {
876  return "";
877  }
879  {
880  return std::string(",label=\" ") + t->toString(FrameTransform::display_transform_mode_t::rotation_as_quaternion) + "\"";
881  }
883  {
884  return std::string(",label=\" ") + t->toString(FrameTransform::display_transform_mode_t::rotation_as_matrix) + "\"";
885  }
887  {
888  return std::string(",label=\" ") + t->toString(FrameTransform::display_transform_mode_t::rotation_as_rpy) + "\"";
889  }
890 
891  yCError(FRAMETRANSFORMCLIENT) << "get_matrix_as_text() invalid option";
892  return "";
893  /*
894  //this is a test to use Latek display
895  std::string s = "\\begin{ bmatrix } \
896  1 & 2 & 3\\ \
897  a & b & c \
898  \\end{ bmatrix }";
899  */
900 }
901 
902 
904 {
905  FrameTransformContainer* p_cont = nullptr;
906  bool br = m_ift_util->getInternalContainer(p_cont);
907  if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return false; }
908 
909  //protect the internal container from concurrent access
910  std::lock_guard<std::recursive_mutex> l(p_cont->m_trf_mutex);
911 
912  //process data
913  std::string dot_string = "digraph G { ";
914  for (auto it = p_cont->begin(); it != p_cont->end(); it++)
915  {
916  FrameTransform t = *it;
917  std::string edge_text = priv_get_matrix_as_text(&t);
918  std::string trf_text = it->src_frame_id + "->" +
919  it->dst_frame_id + " " +
920  "[color = black]";
921  dot_string += trf_text + '\n';
922  }
923 
924  std::string legend = "\n\
925  rankdir=LR\n\
926  node[shape=plaintext]\n\
927  subgraph cluster_01 {\n\
928  label = \"Legend\";\n\
929  key[label=<<table border=\"0\" cellpadding=\"2\" cellspacing=\"0\" cellborder=\"0\">\n\
930  <tr><td align=\"right\" port=\"i1\">YARP timed transform</td></tr>\n\
931  <tr><td align=\"right\" port=\"i2\">YARP static transform</td></tr>\n\
932  </table>>]\n\
933  key2[label=<<table border=\"0\" cellpadding=\"2\" cellspacing=\"0\" cellborder=\"0\">\n\
934  <tr><td port = \"i1\">&nbsp;</td></tr>\n\
935  <tr><td port = \"i2\">&nbsp;</td></tr>\n\
936  </table>>]\n\
937  key:i1:e -> key2:i1:w [color = blue]\n\
938  key:i2:e -> key2:i2:w [color = blue, style=dashed]\n\
939  } }";
940 
941  std::string command_string = "printf '" + dot_string + legend + "' | dot -Tpdf > frames.pdf";
942 #if defined (__linux__)
943  int ret = std::system("dot -V");
944  if (ret != 0)
945  {
946  yCError(FRAMETRANSFORMCLIENT) << "dot executable not found. Please install graphviz.";
947  return false;
948  }
949 
950  yCDebug(FRAMETRANSFORMCLIENT) << "Command string is:" << command_string;
951  ret = std::system(command_string.c_str());
952  if (ret != 0)
953  {
954  yCError(FRAMETRANSFORMCLIENT) << "The following command failed to execute:" << command_string;
955  return false;
956  }
957 #else
958  yCError(FRAMETRANSFORMCLIENT) << "Not yet implemented. Available only Linux";
959  return false;
960 #endif
961  return true;
962 }
float t
#define LOG_THROTTLE_PERIOD
CMRC_DECLARE(frameTransformRC)
constexpr yarp::conf::vocab32_t VOCAB_ERR
Definition: GenericVocabs.h:17
bool ret
yarp::dev::IFrameTransformStorageUtils * m_ift_util
bool waitForTransform(const std::string &target_frame_id, const std::string &source_frame_id, const double &timeout) override
Block until a transform from source_frame_id to target_frame_id is possible or it times out.
bool getAllFrameIds(std::vector< std::string > &ids) override
Gets a vector containing all the registered frames.
bool allFramesAsString(std::string &all_frames) override
Creates a debug string containing the list of all registered frames.
std::string priv_get_matrix_as_text(yarp::math::FrameTransform *t)
bool getParent(const std::string &frame_id, std::string &parent_frame_id) override
Get the parent of a frame.
void threadRelease() override
Release method.
show_transforms_in_diagram_t m_show_transforms_in_diagram
void run() override
Loop function.
bool deleteTransform(const std::string &target_frame_id, const std::string &source_frame_id) override
Deletes a transform between two frames.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool close() override
Close the DeviceDriver.
bool transformQuaternion(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::math::Quaternion &input_quaternion, yarp::math::Quaternion &transformed_quaternion) override
Transform a quaternion into the target frame.
bool transformPose(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_pose, yarp::sig::Vector &transformed_pose) override
Transform a Stamped Pose into the target frame.
bool clear() override
Removes all the registered transforms.
std::vector< broadcast_port_t * > m_array_of_ports
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
bool threadInit() override
Initialization method.
bool transformPoint(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_point, yarp::sig::Vector &transformed_point) override
Transform a point into the target frame.
bool setTransformStatic(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform) override
Register a static transform between two frames.
bool frameExists(const std::string &frame_id) override
Check if a frame exists.
bool setTransform(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform) override
Register a transform between two frames.
bool canTransform(const std::string &target_frame, const std::string &source_frame) override
Test if a transform exists.
bool getTransform(const std::string &target_frame_id, const std::string &source_frame_id, yarp::sig::Matrix &transform) override
Get the transform between two frames.
FrameTransformContainer: A class that contains a vector of frame transformations and exposes yarp::de...
std::recursive_mutex m_trf_mutex
virtual bool getInternalContainer(FrameTransformContainer *&container)=0
void fromRotationMatrix(const yarp::sig::Matrix &R)
Converts a rotation matrix to a quaternion.
Definition: Quaternion.cpp:162
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:73
void addVocab32(yarp::conf::vocab32_t x)
Places a vocabulary item in the bottle, at the end of the list.
Definition: Bottle.cpp:164
size_type size() const
Gets the number of elements in the bottle.
Definition: Bottle.cpp:251
bool read(ConnectionReader &reader) override
Set the bottle's value based on input from a network connection.
Definition: Bottle.cpp:240
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:246
void clear()
Empties the bottle of any objects it contains.
Definition: Bottle.cpp:121
bool write(ConnectionWriter &writer) const override
Output a representation of the bottle to a network connection.
Definition: Bottle.cpp:230
void addString(const char *str)
Places a string in the bottle, at the end of the list.
Definition: Bottle.cpp:170
An interface for reading from a network connection.
virtual ConnectionWriter * getWriter()=0
Gets a way to reply to the message, if possible.
An interface for writing to a network connection.
static bool exists(const std::string &port, bool quiet=true, bool checkVer=true)
Check for a port to be ready and responsive.
Definition: Network.cpp:746
An abstraction for a periodic thread.
A class for storing options and configuration information.
Definition: Property.h:33
Value & find(const std::string &key) const override
Gets a value corresponding to a given keyword.
Definition: Property.cpp:1051
void fromString(const std::string &txt, bool wipe=true)
Interprets a string as a list of properties.
Definition: Property.cpp:1063
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Definition: Property.cpp:1015
bool check(const std::string &key) const override
Check if there exists a property of the given name.
Definition: Property.cpp:1041
void unput(const std::string &key)
Remove the association from the given key to a value, if present.
Definition: Property.cpp:1046
A base class for nested structures that can be searched.
Definition: Searchable.h:63
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
static double nowSystem()
Definition: SystemClock.cpp:34
static void delaySystem(double seconds)
Definition: SystemClock.cpp:29
A single value (typically within a Bottle).
Definition: Value.h:43
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition: Value.cpp:222
std::string toString() const override
Return a standard text representation of the content of the object.
Definition: Value.cpp:356
virtual std::string asString() const
Get string value.
Definition: Value.cpp:234
Result of the parsing of XMLReader.
Definition: XMLReader.h:24
bool parsingIsSuccessful
True if the parsing was successful, false otherwise.
Definition: XMLReader.h:36
Robot robot
If parsingIsSuccessful is true, contains a valid robot instance.
Definition: XMLReader.h:41
XMLReaderResult getRobotFromString(const std::string &filename, const yarp::os::Searchable &config=yarp::os::Property())
Parse the XML description of a robotinterface from a string.
Definition: XMLReader.cpp:140
A class for a Matrix.
Definition: Matrix.h:39
std::string toString(int precision=-1, int width=-1, const char *endRowStr="\n") const
Print matrix to a string.
Definition: Matrix.cpp:171
const Matrix & eye()
Build an identity matrix, don't resize.
Definition: Matrix.cpp:456
void resize(size_t size) override
Resize the vector.
Definition: Vector.h:220
size_t size() const
Definition: Vector.h:321
void push_back(const T &elem)
Push a new element in the vector: size is changed.
Definition: Vector.h:248
void pop_back()
Pop an element out of the vector: size is changed.
Definition: Vector.h:276
#define M_PI
#define yCInfo(component,...)
Definition: LogComponent.h:171
#define yCError(component,...)
Definition: LogComponent.h:213
#define yCAssert(component, x)
Definition: LogComponent.h:240
#define yCTrace(component,...)
Definition: LogComponent.h:84
#define yCWarning(component,...)
Definition: LogComponent.h:192
#define yCDebug(component,...)
Definition: LogComponent.h:128
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:76
#define yCErrorThrottle(component, period,...)
Definition: LogComponent.h:216
Definition: FrameGraph.h:19
std::string to_string(IntegerType x)
Definition: numeric.h:115
For streams capable of holding different kinds of content, check what they actually have.
yarp::sig::Matrix SE3inv(const yarp::sig::Matrix &H)
Returns the inverse of a 4 by 4 rototranslational matrix (defined in Math.h).
Definition: math.cpp:912
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.