Goby v2
modemdriver.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 "goby/common/logger.h"
23 #include "goby/common/logger/term_color.h"
24 #include "goby/common/zeromq_service.h"
25 
26 #include "goby/acomms/connect.h"
27 #include "goby/acomms/modem_driver.h"
28 #include "goby/acomms/modemdriver/driver_exception.h"
29 #include "goby/acomms/modemdriver/iridium_driver.h"
30 #include "goby/acomms/modemdriver/iridium_shore_driver.h"
31 #include "goby/acomms/modemdriver/udp_driver.h"
32 
33 #include "goby/pb/application.h"
34 #include "goby/pb/pb_modem_driver.h"
35 
36 #include "goby/acomms/protobuf/modem_driver_status.pb.h"
37 #include "modemdriver_config.pb.h"
38 
39 using namespace goby::common::logger;
40 
41 namespace goby
42 {
43 namespace acomms
44 {
46 {
47  public:
49  ~ModemDriver();
50 
51  private:
52  void loop();
53 
54  void handle_modem_data_request(protobuf::ModemTransmission* msg);
55  void handle_modem_receive(const protobuf::ModemTransmission& message);
56 
57  void handle_data_response(const protobuf::ModemTransmission& message);
58  void handle_initiate_transmission(const protobuf::ModemTransmission& message);
59 
60  void reset(const ModemDriverException& e);
61 
62  std::string modem_id_str() { return goby::util::as<std::string>(cfg_.driver_cfg().modem_id()); }
63 
64  private:
66 
67  // for PBDriver
68  boost::shared_ptr<goby::common::ZeroMQService> zeromq_service_;
69 
70  // for UDPDriver
71  boost::shared_ptr<boost::asio::io_service> asio_service_;
72 
73  boost::shared_ptr<goby::acomms::ModemDriverBase> driver_;
74 
75  bool data_response_received_;
76  protobuf::ModemTransmission data_response_;
77 
78  bool initiate_transmit_pending_;
79  protobuf::ModemTransmission initiate_transmission_;
80 
81  bool driver_started_;
82 
83  double last_status_time_;
85 };
86 } // namespace acomms
87 } // namespace goby
88 
89 int main(int argc, char* argv[])
90 {
92  goby::run<goby::acomms::ModemDriver>(argc, argv, &cfg);
93 }
94 
95 using goby::glog;
96 
97 goby::acomms::ModemDriver::ModemDriver(protobuf::ModemDriverConfig* cfg)
98  : goby::pb::Application(cfg), cfg_(*cfg), data_response_received_(false),
99  initiate_transmit_pending_(false), driver_started_(false), last_status_time_(0)
100 {
101  glog.is(DEBUG1) && glog << cfg_.DebugString() << std::endl;
102 
103  switch (cfg_.driver_type())
104  {
105  case goby::acomms::protobuf::DRIVER_WHOI_MICROMODEM:
106  driver_.reset(new goby::acomms::MMDriver);
107  break;
108 
109  case goby::acomms::protobuf::DRIVER_PB_STORE_SERVER:
110  zeromq_service_.reset(new goby::common::ZeroMQService);
111  driver_.reset(new goby::pb::PBDriver(zeromq_service_.get()));
112  break;
113 
114  case goby::acomms::protobuf::DRIVER_UDP:
115  asio_service_.reset(new boost::asio::io_service);
116  driver_.reset(new goby::acomms::UDPDriver(asio_service_.get()));
117  break;
118 
119  case goby::acomms::protobuf::DRIVER_IRIDIUM:
120  driver_.reset(new goby::acomms::IridiumDriver);
121 
122  break;
123 
124  case goby::acomms::protobuf::DRIVER_IRIDIUM_SHORE:
125  driver_.reset(new goby::acomms::IridiumShoreDriver);
126  break;
127 
128  default:
129  case goby::acomms::protobuf::DRIVER_NONE:
130  throw(goby::Exception("Invalid/unsupported driver specified"));
131  break;
132  }
133 
134  subscribe(&ModemDriver::handle_initiate_transmission, this,
135  "Tx" + goby::util::as<std::string>(cfg_.driver_cfg().modem_id()));
136 
137  subscribe(&ModemDriver::handle_data_response, this,
138  "DataResponse" + goby::util::as<std::string>(cfg_.driver_cfg().modem_id()));
139 
140  connect(&driver_->signal_receive, this, &ModemDriver::handle_modem_receive);
141 
142  connect(&driver_->signal_data_request, this, &ModemDriver::handle_modem_data_request);
143 
144  status_.set_src(cfg_.driver_cfg().modem_id());
145  status_.set_status(goby::acomms::protobuf::ModemDriverStatus::NOMINAL);
146 }
147 
148 goby::acomms::ModemDriver::~ModemDriver()
149 {
150  if (driver_)
151  driver_->shutdown();
152 }
153 
154 void goby::acomms::ModemDriver::loop()
155 {
156  if (driver_)
157  {
158  try
159  {
160  if (!driver_started_)
161  {
162  driver_->startup(cfg_.driver_cfg());
163  driver_started_ = true;
164  status_.set_status(goby::acomms::protobuf::ModemDriverStatus::NOMINAL);
165  }
166  driver_->do_work();
167  }
168  catch (const ModemDriverException& e)
169  {
170  reset(e);
171  }
172  }
173 
174  double now = goby::common::goby_time<double>();
175  if (last_status_time_ + cfg_.status_period_s() <= now)
176  {
177  status_.set_time(now);
178  publish(status_, "Status" + modem_id_str());
179  last_status_time_ = now;
180  }
181 
182  if (initiate_transmit_pending_)
183  {
184  driver_->handle_initiate_transmission(initiate_transmission_);
185  initiate_transmit_pending_ = false;
186  }
187 }
188 
189 void goby::acomms::ModemDriver::handle_modem_data_request(protobuf::ModemTransmission* msg)
190 {
191  publish(*msg, "DataRequest" + modem_id_str());
192  data_response_received_ = false;
193 
194  double start_time = goby::common::goby_time<double>();
195  while (goby::common::goby_time<double>() < start_time + cfg_.data_request_timeout())
196  {
197  zeromq_service().poll(10000);
198  if (data_response_received_)
199  {
200  *msg = data_response_;
201  break;
202  }
203  }
204  if (!data_response_received_)
205  glog.is(WARN) && glog << "Timeout waiting for response to data request" << std::endl;
206 }
207 
208 void goby::acomms::ModemDriver::handle_modem_receive(const protobuf::ModemTransmission& message)
209 {
210  publish(message, "Rx" + modem_id_str());
211 }
212 
213 void goby::acomms::ModemDriver::handle_data_response(const protobuf::ModemTransmission& message)
214 {
215  data_response_received_ = true;
216  data_response_ = message;
217 }
218 
219 void goby::acomms::ModemDriver::handle_initiate_transmission(
220  const protobuf::ModemTransmission& message)
221 {
222  // wait until we enter next loop to initiate the transmission to avoid calling poll() from within poll()
223  initiate_transmit_pending_ = true;
224  initiate_transmission_ = message;
225 }
226 
227 void goby::acomms::ModemDriver::reset(const ModemDriverException& e)
228 {
229  status_.set_status(e.status());
230  status_.set_n_resets(status_.n_resets() + 1);
231  glog.is(WARN) && glog << "Exception: " << e.what() << std::endl;
232  const int restart_sec = 15;
233  glog.is(WARN) && glog << "Shutting down driver." << std::endl;
234  driver_->shutdown();
235  glog.is(WARN) && glog << "Attempting to restart driver in " << restart_sec << " seconds."
236  << std::endl;
237  sleep(restart_sec);
238  driver_started_ = false;
239 }
Base class provided for users to generate applications that participate in the Goby publish/subscribe...
Definition: application.h:49
provides an API to the WHOI Micro-Modem driver
Definition: mm_driver.h:41
double goby_time< double >()
Returns current UTC time as seconds and fractional seconds since 1970-01-01 00:00:00.
Definition: time.h:130
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.
The global namespace for the Goby project.
simple exception class for goby applications
Definition: exception.h:32