yaml_params.cpp
Go to the documentation of this file.
1 // Methods for converting YAML into XmlRpcValues for the parameter server
2 // Author: Max Schwarz <max.schwarz@ais.uni-bonn.de>
3 
4 #include "yaml_params.h"
5 #include "launch_config.h"
6 #include "substitution.h"
7 #include "substitution_python.h"
8 
9 #include <boost/algorithm/string/predicate.hpp>
10 
11 namespace rosmon
12 {
13 namespace launch
14 {
15 
16 namespace
17 {
18  class XmlRpcValueCreator : public XmlRpc::XmlRpcValue
19  {
20  public:
21  static XmlRpcValueCreator createArray(const std::vector<XmlRpcValue>& values)
22  {
23  XmlRpcValueCreator ret;
24  ret._type = TypeArray;
25  ret._value.asArray = new ValueArray(values);
26 
27  return ret;
28  }
29 
30  static XmlRpcValueCreator createStruct(const std::map<std::string, XmlRpcValue>& members)
31  {
32  XmlRpcValueCreator ret;
33  ret._type = TypeStruct;
34  ret._value.asStruct = new std::map<std::string, XmlRpcValue>(members);
35  return ret;
36  }
37  };
38 }
39 
40 XmlRpc::XmlRpcValue yamlToXmlRpc(const ParseContext& ctx, const YAML::Node& n)
41 {
42  if(n.Type() != YAML::NodeType::Scalar)
43  {
44  switch(n.Type())
45  {
46  case YAML::NodeType::Map:
47  {
48  std::map<std::string, XmlRpc::XmlRpcValue> members;
49  for(YAML::const_iterator it = n.begin(); it != n.end(); ++it)
50  {
51  members[it->first.as<std::string>()] = yamlToXmlRpc(ctx, it->second);
52  }
53  return XmlRpcValueCreator::createStruct(members);
54  }
55  case YAML::NodeType::Sequence:
56  {
57  std::vector<XmlRpc::XmlRpcValue> values;
58  for(YAML::const_iterator it = n.begin(); it != n.end(); ++it)
59  {
60  values.push_back(yamlToXmlRpc(ctx, *it));
61  }
62  return XmlRpcValueCreator::createArray(values);
63  }
64  default:
65  throw ctx.error("Invalid YAML node type");
66  }
67  }
68 
69  // Scalars are tricky, as XmlRpcValue is strongly typed. So we need to
70  // figure out the data type...
71 
72  // Check if a YAML tag is present
73  if(n.Tag() == "tag:yaml.org,2002:int")
74  return XmlRpc::XmlRpcValue(n.as<int>());
75  if(n.Tag() == "tag:yaml.org,2002:float")
76  return XmlRpc::XmlRpcValue(n.as<double>());
77  if(n.Tag() == "tag:yaml.org,2002:bool")
78  return XmlRpc::XmlRpcValue(n.as<bool>());
79  if(n.Tag() == "tag:yaml.org,2002:str")
80  return XmlRpc::XmlRpcValue(n.as<std::string>());
81  if(n.Tag() == "tag:yaml.org,2002:binary")
82  {
83  auto binary = n.as<YAML::Binary>();
84 
85  // XmlRpc API is stupid here and expects a mutable void*, so to keep
86  // things clean we do a copy.
87  std::vector<unsigned char> copy(binary.data(), binary.data()+binary.size());
88 
89  return XmlRpc::XmlRpcValue(copy.data(), copy.size());
90  }
91 
92  // rosparam supports !!degrees and !!radians...
93  if(n.Tag() == "!degrees")
94  {
95  try
96  {
97  return XmlRpc::XmlRpcValue(evaluateROSParamPython(n.as<std::string>()) * M_PI / 180.0);
98  }
99  catch(SubstitutionException& e)
100  {
101  throw ctx.error("{}", e.what());
102  }
103  }
104  if(n.Tag() == "!radians")
105  {
106  try
107  {
108  return XmlRpc::XmlRpcValue(evaluateROSParamPython(n.as<std::string>()));
109  }
110  catch(SubstitutionException& e)
111  {
112  throw ctx.error("{}", e.what());
113  }
114  }
115 
116  // If we have a "non-specific" tag '!', this means that the YAML scalar
117  // is non-plain, i.e. of type seq, map, or str. Since seq and map are
118  // handled above, we assume str in this case.
119  if(n.Tag() == "!")
120  return XmlRpc::XmlRpcValue(n.as<std::string>());
121 
122  // Otherwise, we simply have to try things one by one...
123  try { return XmlRpc::XmlRpcValue(n.as<bool>()); }
124  catch(YAML::Exception&) {}
125 
126  try { return XmlRpc::XmlRpcValue(n.as<int>()); }
127  catch(YAML::Exception&) {}
128 
129  try { return XmlRpc::XmlRpcValue(n.as<double>()); }
130  catch(YAML::Exception&) {}
131 
132  try
133  {
134  std::string str = n.as<std::string>();
135 
136  // rosparam supports deg(...) and rad(...) expressions, so we have to
137  // parse them here.
138 
139  if(boost::starts_with(str, "deg(") && boost::ends_with(str, ")"))
140  {
141  try
142  {
143  return XmlRpc::XmlRpcValue(
144  evaluateROSParamPython(str.substr(4, str.size() - 5)) * M_PI / 180.0
145  );
146  }
147  catch(SubstitutionException& e)
148  {
149  throw ctx.error("{}", e.what());
150  }
151  }
152 
153  if(boost::starts_with(str, "rad(") && boost::ends_with(str, ")"))
154  {
155  try
156  {
157  return XmlRpc::XmlRpcValue(evaluateROSParamPython(str.substr(4, str.size() - 5)));
158  }
159  catch(SubstitutionException& e)
160  {
161  throw ctx.error("{}", e.what());
162  }
163  }
164 
165  return XmlRpc::XmlRpcValue(str);
166  }
167  catch(YAML::Exception&) {}
168 
169  throw ctx.error("Could not convert YAML value");
170 }
171 
172 }
173 }
virtual const char * what() const noexceptoverride
Definition: substitution.h:29
double evaluateROSParamPython(const std::string &input)
Evaluate a deg(...) rosparam expression.
ParseException error(const char *fmt, const Args &...args) const
XmlRpc::XmlRpcValue yamlToXmlRpc(const ParseContext &ctx, const YAML::Node &n)
Definition: yaml_params.cpp:40
float values[3]
Definition: husl.c:29


rosmon_core
Author(s): Max Schwarz
autogenerated on Sat Jan 9 2021 03:35:43