Goby v2
moos_translator.h
1 // Copyright 2009-2018 Toby Schneider (http://gobysoft.org/index.wt/people/toby)
2 // GobySoft, LLC (2013-)
3 // Massachusetts Institute of Technology (2007-2014)
4 // Community contributors (see AUTHORS file)
5 //
6 //
7 // This file is part of the Goby Underwater Autonomy Project Libraries
8 // ("The Goby Libraries").
9 //
10 // The Goby Libraries are free software: you can redistribute them and/or modify
11 // them under the terms of the GNU Lesser General Public License as published by
12 // the Free Software Foundation, either version 2.1 of the License, or
13 // (at your option) any later version.
14 //
15 // The Goby Libraries are distributed in the hope that they will be useful,
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 // GNU Lesser General Public License for more details.
19 //
20 // You should have received a copy of the GNU Lesser General Public License
21 // along with Goby. If not, see <http://www.gnu.org/licenses/>.
22 
23 #ifndef MOOS_TRANSLATOR_H
24 #define MOOS_TRANSLATOR_H
25 
26 #include "goby/moos/moos_header.h"
27 #include "moos_geodesy.h"
28 #include <map>
29 #include <set>
30 #include <string>
31 
32 #include "goby/moos/modem_id_convert.h"
34 #include "goby/moos/protobuf/translator.pb.h"
35 #include "goby/moos/transitional/message_algorithms.h"
36 #include "goby/util/dynamic_protobuf_manager.h"
37 
38 namespace goby
39 {
40 namespace moos
41 {
42 void alg_power_to_dB(transitional::DCCLMessageVal& val_to_mod);
43 void alg_dB_to_power(transitional::DCCLMessageVal& val_to_mod);
44 
45 // applied to "T" (temperature), references are "S" (salinity), then "D" (depth)
46 void alg_TSD_to_soundspeed(transitional::DCCLMessageVal& val,
47  const std::vector<transitional::DCCLMessageVal>& ref_vals);
48 
49 // ref_vals subtracted from val
50 void alg_subtract(transitional::DCCLMessageVal& val,
51  const std::vector<transitional::DCCLMessageVal>& ref_vals);
52 
53 // ref_vals added to val
54 void alg_add(transitional::DCCLMessageVal& val,
55  const std::vector<transitional::DCCLMessageVal>& ref_vals);
56 
57 void alg_angle_0_360(transitional::DCCLMessageVal& angle);
58 void alg_angle_n180_180(transitional::DCCLMessageVal& angle);
59 
60 void alg_to_upper(transitional::DCCLMessageVal& val_to_mod);
61 void alg_to_lower(transitional::DCCLMessageVal& val_to_mod);
62 
63 void alg_abs(transitional::DCCLMessageVal& val_to_mod);
64 void alg_lat2hemisphere_initial(transitional::DCCLMessageVal& val_to_mod);
65 void alg_lon2hemisphere_initial(transitional::DCCLMessageVal& val_to_mod);
66 
67 void alg_unix_time2nmea_time(transitional::DCCLMessageVal& val_to_mod);
68 
69 void alg_lat2nmea_lat(transitional::DCCLMessageVal& val_to_mod);
70 void alg_lon2nmea_lon(transitional::DCCLMessageVal& val_to_mod);
71 
73 {
74  public:
77  double lat_origin = std::numeric_limits<double>::quiet_NaN(),
78  double lon_origin = std::numeric_limits<double>::quiet_NaN(),
79  const std::string& modem_id_lookup_path = "")
80  {
81  initialize(lat_origin, lon_origin, modem_id_lookup_path);
82  if (entry.IsInitialized())
83  add_entry(entry);
84  }
85 
87  const google::protobuf::RepeatedPtrField<goby::moos::protobuf::TranslatorEntry>& entries,
88  double lat_origin = std::numeric_limits<double>::quiet_NaN(),
89  double lon_origin = std::numeric_limits<double>::quiet_NaN(),
90  const std::string& modem_id_lookup_path = "")
91  {
92  initialize(lat_origin, lon_origin, modem_id_lookup_path);
93  add_entry(entries);
94  }
95 
96  void clear_entry(const std::string& protobuf_name) { dictionary_.erase(protobuf_name); }
97 
98  void add_entry(const goby::moos::protobuf::TranslatorEntry& entry)
99  {
100  if (dictionary_.count(entry.protobuf_name()))
101  throw(std::runtime_error("Duplicate translator entry for " + entry.protobuf_name()));
102  dictionary_[entry.protobuf_name()] = entry;
103  }
104 
105  void add_entry(const std::set<goby::moos::protobuf::TranslatorEntry>& entries)
106  {
107  for (std::set<goby::moos::protobuf::TranslatorEntry>::const_iterator it = entries.begin(),
108  n = entries.end();
109  it != n; ++it)
110  { add_entry(*it); } }
111 
112  void add_entry(
113  const google::protobuf::RepeatedPtrField<goby::moos::protobuf::TranslatorEntry>& entries)
114  {
115  for (google::protobuf::RepeatedPtrField<
116  goby::moos::protobuf::TranslatorEntry>::const_iterator it = entries.begin(),
117  n = entries.end();
118  it != n; ++it)
119  { add_entry(*it); } }
120 
121  // ownership of returned pointer goes to caller (must use smart pointer or call delete)
122  template <typename GoogleProtobufMessagePointer, class StringCMOOSMsgMap>
123  GoogleProtobufMessagePointer moos_to_protobuf(const StringCMOOSMsgMap& moos_variables,
124  const std::string& protobuf_name);
125 
126  std::multimap<std::string, CMOOSMsg>
127  protobuf_to_moos(const google::protobuf::Message& protobuf_msg);
128 
129  // advanced: writes to create, instead of publish!
130  std::multimap<std::string, CMOOSMsg>
131  protobuf_to_inverse_moos(const google::protobuf::Message& protobuf_msg);
132 
133  const std::map<std::string, goby::moos::protobuf::TranslatorEntry>& dictionary() const
134  {
135  return dictionary_;
136  }
137 
138  static CMOOSMsg
139  make_moos_msg(const std::string& var, const std::string& str, bool is_binary,
140  goby::moos::protobuf::TranslatorEntry::ParserSerializerTechnique technique,
141  const std::string& pb_name)
142  {
143  if (is_binary)
144  {
145  CMOOSMsg moos_msg(MOOS_NOTIFY, var, str.size(), str.data());
146  moos_msg.SetSourceAux(
147  pb_name + ":" +
148  goby::moos::protobuf::TranslatorEntry::ParserSerializerTechnique_Name(technique));
149  return moos_msg;
150  }
151  else
152  {
153  try
154  {
155  double return_double = boost::lexical_cast<double>(str);
156  return CMOOSMsg(MOOS_NOTIFY, var, return_double);
157  }
158  catch (boost::bad_lexical_cast&)
159  {
160  CMOOSMsg moos_msg(MOOS_NOTIFY, var, str);
161  moos_msg.SetSourceAux(
162  pb_name + ":" +
163  goby::moos::protobuf::TranslatorEntry::ParserSerializerTechnique_Name(
164  technique));
165  return moos_msg;
166  }
167  }
168  }
169 
170  private:
171  void initialize(double lat_origin = std::numeric_limits<double>::quiet_NaN(),
172  double lon_origin = std::numeric_limits<double>::quiet_NaN(),
173  const std::string& modem_id_lookup_path = "");
174 
175  void alg_lat2utm_y(transitional::DCCLMessageVal& mv,
176  const std::vector<transitional::DCCLMessageVal>& ref_vals);
177 
178  void alg_lon2utm_x(transitional::DCCLMessageVal& mv,
179  const std::vector<transitional::DCCLMessageVal>& ref_vals);
180 
181  void alg_utm_x2lon(transitional::DCCLMessageVal& mv,
182  const std::vector<transitional::DCCLMessageVal>& ref_vals);
183 
184  void alg_utm_y2lat(transitional::DCCLMessageVal& mv,
185  const std::vector<transitional::DCCLMessageVal>& ref_vals);
186 
187  void alg_modem_id2name(transitional::DCCLMessageVal& in);
188  void alg_modem_id2type(transitional::DCCLMessageVal& in);
189  void alg_name2modem_id(transitional::DCCLMessageVal& in);
190 
191  private:
192  std::map<std::string, goby::moos::protobuf::TranslatorEntry> dictionary_;
193  CMOOSGeodesy geodesy_;
194  goby::moos::ModemIdConvert modem_lookup_;
195 };
196 
197 inline std::ostream& operator<<(std::ostream& os, const MOOSTranslator& tl)
198 {
199  os << "= Begin MOOSTranslator =\n";
200 
201  int i = 0;
202  for (std::map<std::string, goby::moos::protobuf::TranslatorEntry>::const_iterator
203  it = tl.dictionary().begin(),
204  n = tl.dictionary().end();
205  it != n; ++it)
206  {
207  os << "== Begin Entry " << i << " ==\n"
208  << it->second.DebugString() << "== End Entry " << i << " ==\n";
209 
210  ++i;
211  }
212 
213  os << "= End MOOSTranslator =";
214  return os;
215 }
216 
217 namespace protobuf
218 {
219 inline bool operator<(const protobuf::TranslatorEntry& a, const protobuf::TranslatorEntry& b)
220 {
221  return a.protobuf_name() < b.protobuf_name();
222 }
223 } // namespace protobuf
224 
225 } // namespace moos
226 } // namespace goby
227 
228 inline std::multimap<std::string, CMOOSMsg>
229 goby::moos::MOOSTranslator::protobuf_to_moos(const google::protobuf::Message& protobuf_msg)
230 {
231  std::map<std::string, goby::moos::protobuf::TranslatorEntry>::const_iterator it =
232  dictionary_.find(protobuf_msg.GetDescriptor()->full_name());
233 
234  const std::string& pb_name = protobuf_msg.GetDescriptor()->full_name();
235 
236  if (it == dictionary_.end())
237  throw(std::runtime_error("No TranslatorEntry for Protobuf type: " + pb_name));
238 
239  const goby::moos::protobuf::TranslatorEntry& entry = it->second;
240 
241  std::multimap<std::string, CMOOSMsg> moos_msgs;
242 
243  bool is_binary = false;
244 
245  for (int i = 0, n = entry.publish_size(); i < n; ++i)
246  {
247  std::string return_string;
248  std::string moos_var = entry.publish(i).moos_var();
249 
250  switch (entry.publish(i).technique())
251  {
252  case protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_TEXT_FORMAT:
254  protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_TEXT_FORMAT>::
255  serialize(&return_string, protobuf_msg);
256  break;
257 
258  case protobuf::TranslatorEntry::TECHNIQUE_PREFIXED_PROTOBUF_TEXT_FORMAT:
260  protobuf::TranslatorEntry::TECHNIQUE_PREFIXED_PROTOBUF_TEXT_FORMAT>::
261  serialize(&return_string, protobuf_msg);
262  break;
263 
264  case protobuf::TranslatorEntry::TECHNIQUE_PREFIXED_PROTOBUF_NATIVE_HEX:
266  protobuf::TranslatorEntry::TECHNIQUE_PREFIXED_PROTOBUF_NATIVE_HEX>::
267  serialize(&return_string, protobuf_msg);
268  break;
269 
270  case protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_NATIVE_HEX:
272  protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_NATIVE_HEX>::
273  serialize(&return_string, protobuf_msg);
274  break;
275 
276  case protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_NATIVE_ENCODED:
278  protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_NATIVE_ENCODED>::
279  serialize(&return_string, protobuf_msg);
280  is_binary = true;
281  break;
282 
283  case protobuf::TranslatorEntry::TECHNIQUE_PREFIXED_PROTOBUF_NATIVE_ENCODED:
285  protobuf::TranslatorEntry::TECHNIQUE_PREFIXED_PROTOBUF_NATIVE_ENCODED>::
286  serialize(&return_string, protobuf_msg);
287  is_binary = true;
288  break;
289 
290  case protobuf::TranslatorEntry::TECHNIQUE_COMMA_SEPARATED_KEY_EQUALS_VALUE_PAIRS:
292  protobuf::TranslatorEntry::TECHNIQUE_COMMA_SEPARATED_KEY_EQUALS_VALUE_PAIRS>::
293  serialize(&return_string, protobuf_msg, entry.publish(i).algorithm(),
294  entry.use_short_enum());
295  break;
296 
297  case protobuf::TranslatorEntry::TECHNIQUE_FORMAT:
298  // process moos_variable too (can be a format string itself!)
300  &moos_var, protobuf_msg, entry.publish(i).algorithm(),
301  entry.publish(i).moos_var(), entry.publish(i).repeated_delimiter(),
302  entry.use_short_enum());
303  // now do the format values
305  &return_string, protobuf_msg, entry.publish(i).algorithm(),
306  entry.publish(i).format(), entry.publish(i).repeated_delimiter(),
307  entry.use_short_enum());
308  break;
309  }
310 
311  moos_msgs.insert(
312  std::make_pair(moos_var, make_moos_msg(moos_var, return_string, is_binary,
313  entry.publish(i).technique(), pb_name)));
314  }
315 
316  return moos_msgs;
317 }
318 
319 inline std::multimap<std::string, CMOOSMsg>
320 goby::moos::MOOSTranslator::protobuf_to_inverse_moos(const google::protobuf::Message& protobuf_msg)
321 {
322  std::map<std::string, goby::moos::protobuf::TranslatorEntry>::const_iterator it =
323  dictionary_.find(protobuf_msg.GetDescriptor()->full_name());
324 
325  const std::string& pb_name = protobuf_msg.GetDescriptor()->full_name();
326 
327  if (it == dictionary_.end())
328  throw(std::runtime_error("No TranslatorEntry for Protobuf type: " + pb_name));
329 
330  const goby::moos::protobuf::TranslatorEntry& entry = it->second;
331 
332  std::multimap<std::string, CMOOSMsg> moos_msgs;
333 
334  bool is_binary = false;
335 
336  for (int i = 0, n = entry.create_size(); i < n; ++i)
337  {
338  std::string return_string;
339  std::string moos_var = entry.create(i).moos_var();
340 
341  switch (entry.create(i).technique())
342  {
343  case protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_TEXT_FORMAT:
345  goby::moos::protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_TEXT_FORMAT>::
346  serialize(&return_string, protobuf_msg);
347  break;
348 
349  case protobuf::TranslatorEntry::TECHNIQUE_PREFIXED_PROTOBUF_TEXT_FORMAT:
351  goby::moos::protobuf::TranslatorEntry::
352  TECHNIQUE_PREFIXED_PROTOBUF_TEXT_FORMAT>::serialize(&return_string,
353  protobuf_msg);
354  break;
355 
356  case protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_NATIVE_HEX:
358  goby::moos::protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_NATIVE_HEX>::
359  serialize(&return_string, protobuf_msg);
360  break;
361 
362  case protobuf::TranslatorEntry::TECHNIQUE_PREFIXED_PROTOBUF_NATIVE_HEX:
364  goby::moos::protobuf::TranslatorEntry::TECHNIQUE_PREFIXED_PROTOBUF_NATIVE_HEX>::
365  serialize(&return_string, protobuf_msg);
366  break;
367 
368  case protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_NATIVE_ENCODED:
370  goby::moos::protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_NATIVE_ENCODED>::
371  serialize(&return_string, protobuf_msg);
372  is_binary = true;
373  break;
374 
375  case protobuf::TranslatorEntry::TECHNIQUE_PREFIXED_PROTOBUF_NATIVE_ENCODED:
377  goby::moos::protobuf::TranslatorEntry::
378  TECHNIQUE_PREFIXED_PROTOBUF_NATIVE_ENCODED>::serialize(&return_string,
379  protobuf_msg);
380  is_binary = true;
381  break;
382 
383  case protobuf::TranslatorEntry::TECHNIQUE_COMMA_SEPARATED_KEY_EQUALS_VALUE_PAIRS:
384  {
385  // workaround for bug in protobuf 2.3.0
386  google::protobuf::RepeatedPtrField<
388  empty_algorithms;
389 
391  protobuf::TranslatorEntry::TECHNIQUE_COMMA_SEPARATED_KEY_EQUALS_VALUE_PAIRS>::
392  serialize(&return_string, protobuf_msg, empty_algorithms,
393  entry.use_short_enum());
394  }
395  break;
396 
397  case protobuf::TranslatorEntry::TECHNIQUE_FORMAT:
398  {
399  google::protobuf::RepeatedPtrField<
401  empty_algorithms;
402 
404  &return_string, protobuf_msg, empty_algorithms, entry.create(i).format(),
405  entry.create(i).repeated_delimiter(), entry.use_short_enum());
406  }
407  break;
408  }
409 
410  moos_msgs.insert(
411  std::make_pair(moos_var, make_moos_msg(moos_var, return_string, is_binary,
412  entry.create(i).technique(), pb_name)));
413  }
414 
415  if (entry.trigger().type() == protobuf::TranslatorEntry::Trigger::TRIGGER_PUBLISH)
416  {
417  if (moos_msgs.count(entry.trigger().moos_var()))
418  {
419  // fake the trigger last so that all other inputs get read in first
420  typedef std::multimap<std::string, CMOOSMsg>::iterator It;
421  std::pair<It, It> p = moos_msgs.equal_range(entry.trigger().moos_var());
422  for (It it = p.first; it != p.second; ++it) it->second.m_dfTime = MOOSTime();
423  }
424  else
425  {
426  // add a trigger
427  moos_msgs.insert(std::make_pair(entry.trigger().moos_var(),
428  CMOOSMsg(MOOS_NOTIFY, entry.trigger().moos_var(), "")));
429  }
430  }
431 
432  return moos_msgs;
433 }
434 
435 template <typename GoogleProtobufMessagePointer, class StringCMOOSMsgMap>
436 GoogleProtobufMessagePointer
437 goby::moos::MOOSTranslator::moos_to_protobuf(const StringCMOOSMsgMap& moos_variables,
438  const std::string& protobuf_name)
439 {
440  std::map<std::string, goby::moos::protobuf::TranslatorEntry>::const_iterator it =
441  dictionary_.find(protobuf_name);
442 
443  if (it == dictionary_.end())
444  throw(std::runtime_error("No TranslatorEntry for Protobuf type: " + protobuf_name));
445 
446  const goby::moos::protobuf::TranslatorEntry& entry = it->second;
447 
448  GoogleProtobufMessagePointer msg =
449  goby::util::DynamicProtobufManager::new_protobuf_message<GoogleProtobufMessagePointer>(
450  protobuf_name);
451 
452  if (&*msg == 0)
453  throw(std::runtime_error("Unknown Protobuf type: " + protobuf_name +
454  "; be sure it is compiled in or directly loaded into the "
455  "goby::util::DynamicProtobufManager."));
456 
457  for (int i = 0, n = entry.create_size(); i < n; ++i)
458  {
459  std::multimap<std::string, CMOOSMsg>::const_iterator it =
460  moos_variables.find(entry.create(i).moos_var());
461  std::string source_string =
462  (it == moos_variables.end())
463  ? ""
464  : (it->second.IsString() ? it->second.GetString()
465  : goby::util::as<std::string>(it->second.GetDouble()));
466 
467  switch (entry.create(i).technique())
468  {
469  case protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_TEXT_FORMAT:
471  protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_TEXT_FORMAT>::parse(source_string,
472  &*msg);
473  break;
474 
475  case protobuf::TranslatorEntry::TECHNIQUE_PREFIXED_PROTOBUF_TEXT_FORMAT:
477  protobuf::TranslatorEntry::TECHNIQUE_PREFIXED_PROTOBUF_TEXT_FORMAT>::
478  parse(source_string, &*msg);
479  break;
480 
481  case protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_NATIVE_HEX:
483  protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_NATIVE_HEX>::parse(source_string,
484  &*msg);
485  break;
486 
487  case protobuf::TranslatorEntry::TECHNIQUE_PREFIXED_PROTOBUF_NATIVE_HEX:
489  protobuf::TranslatorEntry::TECHNIQUE_PREFIXED_PROTOBUF_NATIVE_HEX>::
490  parse(source_string, &*msg);
491  break;
492 
493  case protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_NATIVE_ENCODED:
495  protobuf::TranslatorEntry::TECHNIQUE_PROTOBUF_NATIVE_ENCODED>::
496  parse(source_string, &*msg);
497  break;
498 
499  case protobuf::TranslatorEntry::TECHNIQUE_PREFIXED_PROTOBUF_NATIVE_ENCODED:
501  protobuf::TranslatorEntry::TECHNIQUE_PREFIXED_PROTOBUF_NATIVE_ENCODED>::
502  parse(source_string, &*msg);
503  break;
504 
505  case protobuf::TranslatorEntry::TECHNIQUE_COMMA_SEPARATED_KEY_EQUALS_VALUE_PAIRS:
507  protobuf::TranslatorEntry::TECHNIQUE_COMMA_SEPARATED_KEY_EQUALS_VALUE_PAIRS>::
508  parse(source_string, &*msg, entry.create(i).algorithm(),
509  entry.use_short_enum());
510  break;
511 
512  case protobuf::TranslatorEntry::TECHNIQUE_FORMAT:
514  source_string, &*msg, entry.create(i).format(),
515  entry.create(i).repeated_delimiter(), entry.create(i).algorithm(),
516  entry.use_short_enum());
517  break;
518  }
519  }
520 
521  return msg;
522 }
523 
524 #endif
Helpers for MOOS applications for serializing and parsed Google Protocol buffers messages.
The global namespace for the Goby project.