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 
6 #include "FrameTransformServer.h"
7 
8 #include <yarp/os/Log.h>
9 #include <yarp/os/LogComponent.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>
17 CMRC_DECLARE(frameTransformServerRC);
18 
19 using namespace yarp::dev;
20 using namespace yarp::os;
21 using namespace yarp::sig;
22 using namespace yarp::math;
23 
24 namespace {
25 YARP_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);
34  yarp::os::Bottle out;
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();
50  out.addVocab32(VOCAB_ERR);
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 
112  if (!m_robot.enterPhase(yarp::robotinterface::ActionPhaseStartup)) {
113  return false;
114  }
115 
116  if (!m_robot.enterPhase(yarp::robotinterface::ActionPhaseRun)) {
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 
125  m_rpc_InterfaceToUser.setReader(*this);
126 
127  return true;
128 }
129 
131 {
132  this->askToStop();
133  m_rpc_InterfaceToUser.close();
134  yCDebug(FRAMETRANSFORMSERVER, "rpc port closed");
135 
137  m_robot.enterPhase(yarp::robotinterface::ActionPhaseShutdown);
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.
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: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
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.
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.
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
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
#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.