YARP
Yet Another Robot Platform
FrameTransformServer.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
12#include <yarp/math/Math.h>
13
14#include <cmrc/cmrc.hpp>
15#include <mutex>
16#include <fstream>
17CMRC_DECLARE(frameTransformServerRC);
18
19using namespace yarp::dev;
20using namespace yarp::os;
21using namespace yarp::sig;
22using namespace yarp::math;
23
24namespace {
25YARP_LOG_COMPONENT(FRAMETRANSFORMSERVER, "yarp.device.FrameTransformServer")
26}
27#define LOG_THROTTLE_PERIOD 1.0
28
29//------------------------------------------------------------------------------------------------------------------------------
31{
32 std::lock_guard<std::mutex> lock (m_rpc_mutex);
35 bool ok = in.read(connection);
36 if (!ok) {
37 return false;
38 }
39
40 std::string request = in.get(0).asString();
41 if (request == "help")
42 {
43 out.addVocab32("many");
44 out.addString("No RPC commands available");
45 }
46 else
47 {
48 yCError(FRAMETRANSFORMSERVER, "Invalid vocab received in FrameTransformServer");
49 out.clear();
51 out.addString("Invalid command name");
52 }
53
54 yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
55 if (returnToSender != nullptr)
56 {
57 out.write(*returnToSender);
58 }
59 else
60 {
61 yCError(FRAMETRANSFORMSERVER) << "Invalid return to sender";
62 }
63 return true;
64}
65
67{
68 yCWarning(FRAMETRANSFORMSERVER) << "The 'FrameTransformServer' device is experimental and could be modified without any warning";
69
70 std::string cfg_string = config.toString();
72 cfg.fromString(cfg_string);
73
74 std::string configuration_to_open;
75 std::string innerFilePath="config_xml/fts_yarp_only.xml";
76 if(cfg.check("testxml_option"))
77 {
78 innerFilePath = cfg.find("testxml_option").asString();
79 std::ifstream xmlFile(innerFilePath);
80 std::stringstream stream_file;
81 stream_file << xmlFile.rdbuf();
82 configuration_to_open = stream_file.str();
83 }
84 else
85 {
86 auto fs = cmrc::frameTransformServerRC::get_filesystem();
87 if(cfg.check("filexml_option")) { innerFilePath="config_xml/"+cfg.find("filexml_option").toString();}
88 cfg.unput("filexml_option");
89 auto xmlFile = fs.open(innerFilePath);
90 for(const auto& lemma : xmlFile)
91 {
92 configuration_to_open += lemma;
93 }
94 }
95
96 std::string m_local_rpcUser = "/ftServer/rpc";
97 if (cfg.check("local_rpc")) { m_local_rpcUser=cfg.find("local_rpc").toString();}
98 cfg.unput("local_rpc");
99
100 if (config.check("FrameTransform_verbose_debug"))
101 {
102 yarp::os::Value vval = config.find("FrameTransform_verbose_debug");
103 cfg.put("FrameTransform_verbose_debug", vval);
104 }
105
107 yarp::robotinterface::XMLReaderResult result = reader.getRobotFromString(configuration_to_open, cfg);
108 yCAssert(FRAMETRANSFORMSERVER, result.parsingIsSuccessful);
109
110 m_robot = std::move(result.robot);
111
113 return false;
114 }
115
117 return false;
118 }
119
120 if (!m_rpc_InterfaceToUser.open(m_local_rpcUser))
121 {
122 yCError(FRAMETRANSFORMSERVER,"Failed to open rpc port");
123 }
124
126
127 return true;
128}
129
131{
132 this->askToStop();
134 yCDebug(FRAMETRANSFORMSERVER, "rpc port closed");
135
138
139 return true;
140}
141
143 m_period(0.01)
144{
145}
146
148
150{
151 yCTrace(FRAMETRANSFORMSERVER, "Thread started");
152 return true;
153}
154
156{
157 yCTrace(FRAMETRANSFORMSERVER, "Thread stopped");
158}
159
161{
162 //Empty (placeholder for future developments)
163}
CMRC_DECLARE(frameTransformServerRC)
constexpr yarp::conf::vocab32_t VOCAB_ERR
Definition: GenericVocabs.h:17
bool close() override
Close the DeviceDriver.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
void threadRelease() override
Release method.
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
yarp::os::Port m_rpc_InterfaceToUser
yarp::robotinterface::Robot m_robot
bool threadInit() override
Initialization method.
void run() override
Loop function.
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
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.
An abstraction for a periodic thread.
void askToStop()
Stop 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.
A single value (typically within a Bottle).
Definition: Value.h:43
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
bool enterPhase(yarp::robotinterface::ActionPhase phase)
Definition: Robot.cpp:629
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
#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
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.