Goby v2
abc_frontseat_driver.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/util/as.h"
24 
25 #include "abc_frontseat_driver.h"
26 
27 namespace gpb = goby::moos::protobuf;
28 using goby::glog;
30 using namespace goby::common::logger;
31 using namespace goby::common::tcolor;
32 
33 const int allowed_skew = 10;
34 
35 // allows iFrontSeat to load our library
36 extern "C"
37 {
38  FrontSeatInterfaceBase* frontseat_driver_load(iFrontSeatConfig* cfg)
39  {
40  return new AbcFrontSeat(*cfg);
41  }
42 }
43 
44 AbcFrontSeat::AbcFrontSeat(const iFrontSeatConfig& cfg)
45  : FrontSeatInterfaceBase(cfg), abc_config_(cfg.GetExtension(abc_config)),
46  tcp_(abc_config_.tcp_address(), abc_config_.tcp_port()), frontseat_providing_data_(false),
47  last_frontseat_data_time_(0), frontseat_state_(gpb::FRONTSEAT_NOT_CONNECTED)
48 {
49  tcp_.start();
50 
51  // wait for up to 10 seconds for a connection
52  // in a real driver, should keep trying to reconnect (maybe with a backoff).
53  int timeout = 10, i = 0;
54  while (!tcp_.active() && i < timeout)
55  {
56  ++i;
57  sleep(1);
58  }
59 }
60 
61 void AbcFrontSeat::loop()
62 {
63  check_connection_state();
64  try_receive();
65 
66  // if we haven't gotten data for a while, set this boolean so that the
67  // FrontSeatInterfaceBase class knows
68  if (goby_time<double>() > last_frontseat_data_time_ + allowed_skew)
69  frontseat_providing_data_ = false;
70 } // loop
71 
72 void AbcFrontSeat::check_connection_state()
73 {
74  // check the connection state
75  if (!tcp_.active())
76  {
77  // in a real driver, change this to try to reconnect (see Bluefin driver example)
78  glog.is(DIE) && glog << "Connection to FrontSeat failed: " << abc_config_.tcp_address()
79  << ":" << abc_config_.tcp_port() << std::endl;
80  }
81  else
82  {
83  // on connection, send the START command to initialize the simulator
84  if (frontseat_state_ == gpb::FRONTSEAT_NOT_CONNECTED)
85  {
86  glog.is(VERBOSE) && glog << "Connected to ABC Simulator." << std::endl;
87  frontseat_state_ = gpb::FRONTSEAT_IDLE;
88  std::stringstream start_ss;
89  start_ss << "START,"
90  << "LAT:" << abc_config_.start().lat() << ","
91  << "LON:" << abc_config_.start().lon() << ","
92  << "DURATION:" << abc_config_.start().duration();
93  write(start_ss.str());
94  }
95  }
96 }
97 
98 void AbcFrontSeat::try_receive()
99 {
100  std::string in;
101  while (tcp_.readline(&in))
102  {
103  boost::trim(in);
104  try
105  {
106  process_receive(in);
107  }
108  catch (std::exception& e)
109  {
110  glog.is(DEBUG1) && glog << warn << "Failed to handle message: " << e.what()
111  << std::endl;
112  }
113  }
114 }
115 
116 void AbcFrontSeat::process_receive(const std::string& s)
117 {
118  gpb::FrontSeatRaw raw_msg;
119  raw_msg.set_raw(s);
120  signal_raw_from_frontseat(raw_msg);
121 
122  std::map<std::string, std::string> parsed;
123  parse_in(s, &parsed);
124 
125  // frontseat state message
126  if (parsed["KEY"] == "CTRL")
127  {
128  if (parsed["STATE"] == "PAYLOAD")
129  frontseat_state_ = gpb::FRONTSEAT_ACCEPTING_COMMANDS;
130  else if (parsed["STATE"] == "AUV")
131  frontseat_state_ = gpb::FRONTSEAT_IN_CONTROL;
132  else
133  frontseat_state_ = gpb::FRONTSEAT_IDLE;
134  }
135  // frontseat navigation message
136  else if (parsed["KEY"] == "NAV")
137  {
139  goby::moos::protobuf::NodeStatus& status = *data.mutable_node_status();
140 
141  glog.is(VERBOSE) && glog << "Got NAV update: " << s << std::endl;
142  status.mutable_pose()->set_heading(goby::util::as<double>(parsed["HEADING"]));
143  status.set_speed(goby::util::as<double>(parsed["SPEED"]));
144  status.mutable_global_fix()->set_depth(goby::util::as<double>(parsed["DEPTH"]));
145  status.mutable_global_fix()->set_lon(goby::util::as<double>(parsed["LON"]));
146  status.mutable_global_fix()->set_lat(goby::util::as<double>(parsed["LAT"]));
147 
148  // calculates the local fix (X, Y, Z) from global fix
149  compute_missing(&status);
150 
151  signal_data_from_frontseat(data);
152 
153  frontseat_providing_data_ = true;
154  last_frontseat_data_time_ = goby_time<double>();
155  }
156  // frontseat response to our command message
157  else if (parsed["KEY"] == "CMD")
158  {
159  if (last_request_.response_requested())
160  {
161  gpb::CommandResponse response;
162  response.set_request_successful(parsed["RESULT"] == "OK");
163  response.set_request_id(last_request_.request_id());
164  signal_command_response(response);
165  }
166  }
167  else
168  {
169  glog.is(VERBOSE) && glog << "Unknown message from frontseat: " << s << std::endl;
170  }
171 }
172 
173 void AbcFrontSeat::send_command_to_frontseat(const gpb::CommandRequest& command)
174 {
175  if (command.has_desired_course())
176  {
177  std::stringstream cmd_ss;
178  const goby::moos::protobuf::DesiredCourse& desired_course = command.desired_course();
179  cmd_ss << "CMD,"
180  << "HEADING:" << desired_course.heading() << ","
181  << "SPEED:" << desired_course.speed() << ","
182  << "DEPTH:" << desired_course.depth();
183 
184  write(cmd_ss.str());
185  last_request_ = command;
186  }
187  else
188  {
189  glog.is(VERBOSE) && glog << "Unhandled command: " << command.ShortDebugString()
190  << std::endl;
191  }
192 
193 } // send_command_to_frontseat
194 
195 void AbcFrontSeat::send_data_to_frontseat(const gpb::FrontSeatInterfaceData& data)
196 {
197  // ABC driver doesn't have any data to sent to the frontseat
198 } // send_data_to_frontseat
199 
200 void AbcFrontSeat::send_raw_to_frontseat(const gpb::FrontSeatRaw& data)
201 {
202  write(data.raw());
203 } // send_raw_to_frontseat
204 
205 bool AbcFrontSeat::frontseat_providing_data() const
206 {
207  return frontseat_providing_data_;
208 } // frontseat_providing_data
209 
210 goby::moos::protobuf::FrontSeatState AbcFrontSeat::frontseat_state() const
211 {
212  return frontseat_state_;
213 } // frontseat_state
214 
215 void AbcFrontSeat::write(const std::string& s)
216 {
217  gpb::FrontSeatRaw raw_msg;
218  raw_msg.set_raw(s);
219  signal_raw_to_frontseat(raw_msg);
220 
221  tcp_.write(s + "\r\n");
222 }
223 
224 // transforms a string of format "{field0},{key1}:{field1},{key2}:{field2} into a map of
225 // "KEY"=>{field0}
226 // {key1}=>{field1}
227 // {key2}=>{field2}
228 void AbcFrontSeat::parse_in(const std::string& in, std::map<std::string, std::string>* out)
229 {
230  std::vector<std::string> comma_split;
231  boost::split(comma_split, in, boost::is_any_of(","));
232  out->insert(std::make_pair("KEY", comma_split.at(0)));
233  for (int i = 1, n = comma_split.size(); i < n; ++i)
234  {
235  std::vector<std::string> colon_split;
236  boost::split(colon_split, comma_split[i], boost::is_any_of(":"));
237  out->insert(std::make_pair(colon_split.at(0), colon_split.at(1)));
238  }
239 }
Contains functions for adding color to Terminal window streams.
Definition: term_color.h:54
ReturnType goby_time()
Returns current UTC time as a boost::posix_time::ptime.
Definition: time.h:104
common::FlexOstream glog
Access the Goby logger through this object.