Goby v2
legacy_translator.cpp
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 //
5 //
6 // This file is part of the Goby Underwater Autonomy Project Binaries
7 // ("The Goby Binaries").
8 //
9 // The Goby Binaries are free software: you can redistribute them and/or modify
10 // them under the terms of the GNU General Public License as published by
11 // the Free Software Foundation, either version 2 of the License, or
12 // (at your option) any later version.
13 //
14 // The Goby Binaries are distributed in the hope that they will be useful,
15 // but WITHOUT ANY WARRANTY; without even the implied warranty of
16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 // GNU General Public License for more details.
18 //
19 // You should have received a copy of the GNU General Public License
20 // along with Goby. If not, see <http://www.gnu.org/licenses/>.
21 
22 #include <boost/assign.hpp>
23 
24 #include "goby/acomms/connect.h"
25 
26 #include "iFrontSeat.h"
27 #include "legacy_translator.h"
28 
29 #include "goby/moos/frontseat/bluefin/bluefin.pb.h"
30 
31 namespace gpb = goby::moos::protobuf;
32 using goby::glog;
33 using namespace goby::common::logger;
34 
35 FrontSeatLegacyTranslator::FrontSeatLegacyTranslator(iFrontSeat* fs) : ifs_(fs), request_id_(0)
36 {
37  using boost::assign::operator+=;
38 
39  if (ifs_->cfg_.legacy_cfg().subscribe_ctd())
40  {
41  std::vector<std::string> ctd_params;
42  ctd_params += "CONDUCTIVITY", "TEMPERATURE", "PRESSURE", "SALINITY";
43  for (std::vector<std::string>::const_iterator it = ctd_params.begin(),
44  end = ctd_params.end();
45  it != end; ++it)
46  { ifs_->subscribe("CTD_" + *it, &FrontSeatLegacyTranslator::handle_mail_ctd, this, 1); }
47 
48  ctd_sample_.set_temperature(std::numeric_limits<double>::quiet_NaN());
49  ctd_sample_.set_pressure(std::numeric_limits<double>::quiet_NaN());
50  ctd_sample_.set_salinity(std::numeric_limits<double>::quiet_NaN());
51  ctd_sample_.set_lat(std::numeric_limits<double>::quiet_NaN());
52  ctd_sample_.set_lon(std::numeric_limits<double>::quiet_NaN());
53  // we'll let FrontSeatInterfaceBase::compute_missing() give us density, sound speed & depth
54  }
55 
56  if (ifs_->cfg_.legacy_cfg().subscribe_desired())
57  {
58  std::vector<std::string> desired_params;
59  desired_params += "HEADING", "SPEED", "DEPTH", "PITCH", "ROLL", "Z_RATE", "ALTITUDE";
60  for (std::vector<std::string>::const_iterator it = desired_params.begin(),
61  end = desired_params.end();
62  it != end; ++it)
63  {
64  ifs_->subscribe("DESIRED_" + *it,
65  &FrontSeatLegacyTranslator::handle_mail_desired_course, this, 1);
66  }
67  }
68 
69  if (ifs_->cfg_.legacy_cfg().subscribe_acomms_raw())
70  {
71  ifs_->subscribe(
72  "ACOMMS_RAW_INCOMING",
73  boost::bind(&FrontSeatLegacyTranslator::handle_mail_modem_raw, this, _1, INCOMING));
74  ifs_->subscribe(
75  "ACOMMS_RAW_OUTGOING",
76  boost::bind(&FrontSeatLegacyTranslator::handle_mail_modem_raw, this, _1, OUTGOING));
77  }
78 
79  if (ifs_->cfg_.legacy_cfg().pub_sub_bf_commands())
80  {
81  ifs_->subscribe("BUOYANCY_CONTROL",
82  &FrontSeatLegacyTranslator::handle_mail_buoyancy_control, this);
83  ifs_->subscribe("TRIM_CONTROL", &FrontSeatLegacyTranslator::handle_mail_trim_control, this);
84  ifs_->subscribe("FRONTSEAT_BHVOFF",
85  &FrontSeatLegacyTranslator::handle_mail_frontseat_bhvoff, this);
86  ifs_->subscribe("FRONTSEAT_SILENT",
87  &FrontSeatLegacyTranslator::handle_mail_frontseat_silent, this);
88  ifs_->subscribe("BACKSEAT_ABORT", &FrontSeatLegacyTranslator::handle_mail_backseat_abort,
89  this);
90  }
91 
92  goby::acomms::connect(&ifs_->frontseat_->signal_data_from_frontseat, this,
93  &FrontSeatLegacyTranslator::handle_driver_data_from_frontseat);
94 
95  if (ifs_->cfg_.legacy_cfg().publish_fs_bs_ready())
96  {
97  goby::acomms::connect(&ifs_->frontseat_->signal_state_change, this,
98  &FrontSeatLegacyTranslator::set_fs_bs_ready_flags);
99  }
100 }
101 void FrontSeatLegacyTranslator::handle_driver_data_from_frontseat(
103 {
104  if (data.has_node_status() && ifs_->cfg_.legacy_cfg().publish_nav())
105  {
106  const goby::moos::protobuf::NodeStatus& status = data.node_status();
107 
108  ctd_sample_.set_lat(status.global_fix().lat());
109  ctd_sample_.set_lon(status.global_fix().lon());
110 
111  // post NAV_*
112  ifs_->publish("NAV_X", status.local_fix().x());
113  ifs_->publish("NAV_Y", status.local_fix().y());
114  ifs_->publish("NAV_LAT", status.global_fix().lat());
115  ifs_->publish("NAV_LONG", status.global_fix().lon());
116 
117  if (status.local_fix().has_z())
118  ifs_->publish("NAV_Z", status.local_fix().z());
119  if (status.global_fix().has_depth())
120  ifs_->publish("NAV_DEPTH", status.global_fix().depth());
121 
122  const double pi = 3.14159;
123  if (status.pose().has_heading())
124  {
125  double yaw = -status.pose().heading() * pi / 180.0;
126  while (yaw < -pi) yaw += 2 * pi;
127  while (yaw >= pi) yaw -= 2 * pi;
128  ifs_->publish("NAV_YAW", yaw);
129  ifs_->publish("NAV_HEADING", status.pose().heading());
130  }
131 
132  ifs_->publish("NAV_SPEED", status.speed());
133 
134  if (status.pose().has_pitch())
135  ifs_->publish("NAV_PITCH", status.pose().pitch() * pi / 180.0);
136  if (status.pose().has_roll())
137  ifs_->publish("NAV_ROLL", status.pose().roll() * pi / 180.0);
138 
139  if (status.global_fix().has_altitude())
140  ifs_->publish("NAV_ALTITUDE", status.global_fix().altitude());
141 
142  // surface for GPS variable
143  if (status.global_fix().lat_source() == goby::moos::protobuf::GPS &&
144  status.global_fix().lon_source() == goby::moos::protobuf::GPS)
145  {
146  std::stringstream ss;
147  ss << "Timestamp=" << std::setprecision(15) << status.time();
148  ifs_->publish("GPS_UPDATE_RECEIVED", ss.str());
149  }
150  }
151 
152  if (data.HasExtension(gpb::bluefin_data) && ifs_->cfg_.legacy_cfg().pub_sub_bf_commands())
153  {
154  const gpb::BluefinExtraData& bf_data = data.GetExtension(gpb::bluefin_data);
155  if (bf_data.has_trim_status())
156  {
157  std::stringstream trim_report;
158  const gpb::TrimStatus& trim = bf_data.trim_status();
159 
160  trim_report << "status=" << static_cast<int>(trim.status())
161  << ",error=" << static_cast<int>(trim.error())
162  << ",trim_pitch=" << trim.pitch_trim_degrees()
163  << ",trim_roll=" << trim.roll_trim_degrees();
164 
165  ifs_->publish("TRIM_REPORT", trim_report.str());
166  }
167 
168  if (bf_data.has_buoyancy_status())
169  {
170  std::stringstream buoyancy_report;
171  const gpb::BuoyancyStatus& buoyancy = bf_data.buoyancy_status();
172  buoyancy_report << "status=" << static_cast<int>(buoyancy.status())
173  << ",error=" << static_cast<int>(buoyancy.error())
174  << ",buoyancy=" << buoyancy.buoyancy_newtons();
175  ifs_->publish("BUOYANCY_REPORT", buoyancy_report.str());
176  }
177  }
178 }
179 
180 void FrontSeatLegacyTranslator::handle_mail_ctd(const CMOOSMsg& msg)
181 {
182  const std::string& key = msg.GetKey();
183  if (key == "CTD_CONDUCTIVITY")
184  {
185  // - should be in siemens/meter, assuming it's a SeaBird 49 SBE
186  // using iCTD. Thus, no conversion needed (see ctd_sample.proto)
187  // - we need to clean up this units conversion
188 
189  ctd_sample_.set_conductivity(msg.GetDouble());
190  }
191  else if (key == "CTD_TEMPERATURE")
192  {
193  // - degrees C is a safe assumption
194  ctd_sample_.set_temperature(msg.GetDouble());
195 
196  // We'll use the variable to key postings, since it's
197  // always present (even in simulations)
198  ctd_sample_.set_time(msg.GetTime());
200  *data.mutable_ctd_sample() = ctd_sample_;
201  ifs_->frontseat_->compute_missing(data.mutable_ctd_sample());
202 
203  ifs_->publish_pb(ifs_->cfg_.moos_var().prefix() + ifs_->cfg_.moos_var().data_to_frontseat(),
204  data);
205  }
206  else if (key == "CTD_PRESSURE")
207  {
208  // - MOOS var is decibars assuming it's a SeaBird 49 SBE using iCTD.
209  // - GLINT10 data supports this assumption
210  // - CTDSample uses Pascals
211 
212  const double dBar_TO_Pascal = 1e4; // 1 dBar == 10000 Pascals
213  ctd_sample_.set_pressure(msg.GetDouble() * dBar_TO_Pascal);
214  }
215  else if (key == "CTD_SALINITY")
216  {
217  // salinity is standardized to practical salinity scale
218  ctd_sample_.set_salinity(msg.GetDouble());
219  }
220 }
221 
222 void FrontSeatLegacyTranslator::handle_mail_desired_course(const CMOOSMsg& msg)
223 {
224  const std::string& key = msg.GetKey();
225  if (key == "DESIRED_SPEED")
226  {
227  desired_course_.set_speed(msg.GetDouble());
228  desired_course_.set_time(msg.GetTime());
229  gpb::CommandRequest command;
230  *command.mutable_desired_course() = desired_course_;
231  command.set_response_requested(true);
232  command.set_request_id(LEGACY_REQUEST_IDENTIFIER + request_id_++);
233 
234  ifs_->publish_pb(ifs_->cfg_.moos_var().prefix() + ifs_->cfg_.moos_var().command_request(),
235  command);
236  }
237  else if (key == "DESIRED_HEADING")
238  {
239  desired_course_.set_heading(msg.GetDouble());
240  }
241  else if (key == "DESIRED_DEPTH")
242  {
243  desired_course_.set_depth(msg.GetDouble());
244  }
245  else if (key == "DESIRED_PITCH")
246  {
247  desired_course_.set_pitch(msg.GetDouble());
248  }
249  else if (key == "DESIRED_ROLL")
250  {
251  desired_course_.set_roll(msg.GetDouble());
252  }
253  else if (key == "DESIRED_Z_RATE")
254  {
255  desired_course_.set_z_rate(msg.GetDouble());
256  }
257  else if (key == "DESIRED_ALTITUDE")
258  {
259  desired_course_.set_altitude(msg.GetDouble());
260  }
261 }
262 
263 void FrontSeatLegacyTranslator::handle_mail_modem_raw(const CMOOSMsg& msg,
264  ModemRawDirection direction)
265 {
267  parse_for_moos(msg.GetString(), &raw);
269 
270  switch (direction)
271  {
272  case OUTGOING:
273  *data.MutableExtension(gpb::bluefin_data)->mutable_micro_modem_raw_out() = raw;
274  break;
275  case INCOMING:
276  *data.MutableExtension(gpb::bluefin_data)->mutable_micro_modem_raw_in() = raw;
277  break;
278  }
279 
280  ifs_->publish_pb(ifs_->cfg_.moos_var().prefix() + ifs_->cfg_.moos_var().data_to_frontseat(),
281  data);
282 }
283 
284 void FrontSeatLegacyTranslator::set_fs_bs_ready_flags(goby::moos::protobuf::InterfaceState state)
285 {
286  goby::moos::protobuf::FrontSeatInterfaceStatus status = ifs_->frontseat_->status();
287  if (status.frontseat_state() == gpb::FRONTSEAT_ACCEPTING_COMMANDS)
288  ifs_->publish("FRONTSEAT_READY", 1);
289  else
290  ifs_->publish("FRONTSEAT_READY", 0);
291 
292  if (status.helm_state() == gpb::HELM_DRIVE)
293  ifs_->publish("BACKSEAT_READY", 1);
294  else
295  ifs_->publish("BACKSEAT_READY", 0);
296 }
297 
298 void FrontSeatLegacyTranslator::handle_mail_buoyancy_control(const CMOOSMsg& msg)
299 {
300  if (goby::util::as<bool>(boost::trim_copy(msg.GetString())))
301  {
302  gpb::CommandRequest command;
303  command.set_response_requested(true);
304  command.set_request_id(LEGACY_REQUEST_IDENTIFIER + request_id_++);
305  gpb::BluefinExtraCommands* bluefin_command = command.MutableExtension(gpb::bluefin_command);
306  bluefin_command->set_command(gpb::BluefinExtraCommands::BUOYANCY_ADJUST);
307 
308  publish_command(command);
309  }
310 }
311 
312 void FrontSeatLegacyTranslator::handle_mail_trim_control(const CMOOSMsg& msg)
313 {
314  if (goby::util::as<bool>(boost::trim_copy(msg.GetString())))
315  {
316  gpb::CommandRequest command;
317  command.set_response_requested(true);
318  command.set_request_id(LEGACY_REQUEST_IDENTIFIER + request_id_++);
319  gpb::BluefinExtraCommands* bluefin_command = command.MutableExtension(gpb::bluefin_command);
320  bluefin_command->set_command(gpb::BluefinExtraCommands::TRIM_ADJUST);
321 
322  publish_command(command);
323  }
324 }
325 
326 void FrontSeatLegacyTranslator::handle_mail_frontseat_bhvoff(const CMOOSMsg& msg)
327 {
328  if (goby::util::as<bool>(boost::trim_copy(msg.GetString())))
329  {
330  gpb::CommandRequest command;
331  command.set_response_requested(true);
332  command.set_request_id(LEGACY_REQUEST_IDENTIFIER + request_id_++);
333  gpb::BluefinExtraCommands* bluefin_command = command.MutableExtension(gpb::bluefin_command);
334  bluefin_command->set_command(gpb::BluefinExtraCommands::CANCEL_CURRENT_BEHAVIOR);
335 
336  publish_command(command);
337  }
338 }
339 
340 void FrontSeatLegacyTranslator::handle_mail_frontseat_silent(const CMOOSMsg& msg)
341 {
342  gpb::CommandRequest command;
343  command.set_response_requested(true);
344  command.set_request_id(LEGACY_REQUEST_IDENTIFIER + request_id_++);
345  gpb::BluefinExtraCommands* bluefin_command = command.MutableExtension(gpb::bluefin_command);
346  bluefin_command->set_command(gpb::BluefinExtraCommands::SILENT_MODE);
347 
348  if (goby::util::as<bool>(boost::trim_copy(msg.GetString())))
349  bluefin_command->set_silent_mode(gpb::BluefinExtraCommands::SILENT);
350  else
351  bluefin_command->set_silent_mode(gpb::BluefinExtraCommands::NORMAL);
352 
353  publish_command(command);
354 }
355 
356 void FrontSeatLegacyTranslator::handle_mail_backseat_abort(const CMOOSMsg& msg)
357 {
358  gpb::CommandRequest command;
359  command.set_response_requested(true);
360  command.set_request_id(LEGACY_REQUEST_IDENTIFIER + request_id_++);
361  gpb::BluefinExtraCommands* bluefin_command = command.MutableExtension(gpb::bluefin_command);
362  bluefin_command->set_command(gpb::BluefinExtraCommands::ABORT_MISSION);
363 
364  if (goby::util::as<int>(msg.GetDouble()) == 0)
365  bluefin_command->set_abort_reason(gpb::BluefinExtraCommands::SUCCESSFUL_MISSION);
366  else
367  bluefin_command->set_abort_reason(gpb::BluefinExtraCommands::ABORT_WITH_ERRORS);
368 
369  publish_command(command);
370 }
371 
372 void FrontSeatLegacyTranslator::publish_command(const gpb::CommandRequest& command)
373 {
374  ifs_->publish_pb(ifs_->cfg_.moos_var().prefix() + ifs_->cfg_.moos_var().command_request(),
375  command);
376 }
void parse_for_moos(const std::string &in, google::protobuf::Message *msg)
Parses the string in to Google Protocol Buffers message msg. All errors are written to the goby::util...
void connect(Signal *signal, Slot slot)
connect a signal to a slot (e.g. function pointer)
Definition: connect.h:36
common::FlexOstream glog
Access the Goby logger through this object.