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
10
11#include <yarp/os/Log.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
20CMRC_DECLARE(frameTransformRC);
21
22using namespace yarp::dev;
23using namespace yarp::os;
24using namespace yarp::sig;
25using namespace yarp::math;
26
27namespace {
28YARP_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);
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";
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 }
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();
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();
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
344 return false;
345 }
346
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
381
382 return true;
383}
384
386{
387 this->askToStop();
389 yCDebug(FRAMETRANSFORMCLIENT, "rpc port closed");
390
393
394 return true;
395}
396
397bool 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
414FrameTransformClient::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
474bool 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
489bool 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
509bool 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
546bool 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
568bool 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
588bool 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
621bool 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
656bool 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
694bool 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
728bool 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
738bool 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
758bool 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
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
792bool 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
806bool 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
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.
yarp::dev::IFrameTransformStorageSet * m_ift_set
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.
yarp::robotinterface::Robot m_robot
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.
yarp::os::Port m_rpc_InterfaceToUser
bool canTransform(const std::string &target_frame, const std::string &source_frame) override
Test if a transform exists.
yarp::dev::IFrameTransformStorageGet * m_ift_get
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 deleteTransform(std::string src, std::string dst)=0
Delete a single transform in the storage.
virtual bool setTransform(const yarp::math::FrameTransform &transform)=0
Save a frame transform in a storage.
virtual bool clearAll()=0
Delete all transforms in a storage.
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:64
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.
bool setPeriod(double period)
Set the (new) period of the thread.
void askToStop()
Stop the thread.
bool start()
Call this to start the thread.
void setReader(PortReader &reader) override
Set an external reader for port data.
Definition: Port.cpp:511
void close() override
Stop port activity.
Definition: Port.cpp:363
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
Definition: Port.cpp:79
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:56
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.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
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
yarp::dev::PolyDriver * driver() const
Definition: Device.cpp:336
bool hasParam(const std::string &name) const
Definition: Robot.cpp:791
bool hasDevice(const std::string &name) const
Definition: Robot.cpp:597
bool enterPhase(yarp::robotinterface::ActionPhase phase)
Definition: Robot.cpp:629
std::string findParam(const std::string &name) const
Definition: Robot.cpp:796
Device & device(const std::string &name)
Definition: Robot.cpp:567
Result of the parsing of yarp::robotinterface::XMLReader.
Definition: XMLReader.h:26
bool parsingIsSuccessful
True if the parsing was successful, false otherwise.
Definition: XMLReader.h:38
Robot robot
If parsingIsSuccessful is true, contains a valid robot instance.
Definition: XMLReader.h:43
Class to read an XML file.
Definition: XMLReader.h:52
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.