remote_interface.cc
Go to the documentation of this file.
1 /*
2  * This file is part of the rc_dynamics_api package.
3  *
4  * Copyright (c) 2017 Roboception GmbH
5  * All rights reserved
6  *
7  * Author: Christian Emmerich
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * 1. Redistributions of source code must retain the above copyright notice,
13  * this list of conditions and the following disclaimer.
14  *
15  * 2. Redistributions in binary form must reproduce the above copyright notice,
16  * this list of conditions and the following disclaimer in the documentation
17  * and/or other materials provided with the distribution.
18  *
19  * 3. Neither the name of the copyright holder nor the names of its contributors
20  * may be used to endorse or promote products derived from this software without
21  * specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
24  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
25  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
26  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
27  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
28  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
29  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
30  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
31  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
32  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
36 #include "remote_interface.h"
38 
39 #include "json.hpp"
40 #include <cpr/cpr.h>
41 #include <regex>
42 
43 using namespace std;
45 
46 namespace rc
47 {
48 namespace dynamics
49 {
50 // Definitions of static const members
51 const std::string RemoteInterface::State::IDLE = "IDLE";
52 const std::string RemoteInterface::State::RUNNING = "RUNNING";
53 const std::string RemoteInterface::State::FATAL = "FATAL";
54 const std::string RemoteInterface::State::WAITING_FOR_INS = "WAITING_FOR_INS";
55 const std::string RemoteInterface::State::WAITING_FOR_INS_AND_SLAM = "WAITING_FOR_INS_AND_SLAM";
56 const std::string RemoteInterface::State::WAITING_FOR_SLAM = "WAITING_FOR_SLAM";
57 const std::string RemoteInterface::State::RUNNING_WITH_SLAM = "RUNNING_WITH_SLAM";
58 const std::string RemoteInterface::State::UNKNOWN = "UNKNOWN";
59 
60 string toString(cpr::Response resp)
61 {
62  stringstream s;
63  s << "status code: " << resp.status_code << endl
64  << "url: " << resp.url << endl
65  << "text: " << resp.text << endl
66  << "error: " << resp.error.message;
67  return s.str();
68 }
69 
70 string toString(list<string> list)
71 {
72  stringstream s;
73  s << "[";
74  for (auto it = list.begin(); it != list.end();)
75  {
76  s << *it;
77  if (++it != list.end())
78  {
79  s << ", ";
80  }
81  }
82  s << "]";
83  return s.str();
84 }
85 
86 void handleCPRResponse(cpr::Response r)
87 {
88  switch (r.status_code) {
89  case 200:
90  return;
91  case 429:
93  default:
94  throw runtime_error(toString(r));
95  }
96 }
97 
98 namespace {
99 
100  vector<int> wait_before_retry = { 50, 200, 500, 1000, 2000};
101 
102  // Wrapper around cpr::Get requests which does retries in case of 429 response
103  cpr::Response cprGetWithRetry(cpr::Url url, cpr::Timeout timeout) {
104  for (int retry : wait_before_retry) {
105  auto response = cpr::Get(url, timeout, cpr::Header{ { "accept", "application/json" }});
106  if (response.status_code == 429) {
107  cout << "WARNING: Got http code 429 (too many requests) on "
108  << url << ". Retrying in " << retry << "ms..." << endl;
109  usleep(1000 * retry);
110  continue;
111  }
112  return response;
113  }
115  }
116 
117  // Wrapper around cpr::Put requests which does retries in case of 429 response
118  cpr::Response cprPutWithRetry(cpr::Url url, cpr::Timeout timeout, cpr::Body body = cpr::Body{}) {
119 
120  // we need different headers if body is empty or not
121  cpr::Header header;
122  if (body == cpr::Body{}) {
123  header = cpr::Header{ { "accept", "application/json" }};
124  } else {
125  header = cpr::Header{ { "accept", "application/json" }, { "Content-Type", "application/json" }};
126  }
127 
128  for (int retry : wait_before_retry) {
129  auto response = cpr::Put(url, timeout, body, header);
130  if (response.status_code==429) {
131  cout << "WARNING: Got http code 429 (too many requests) on "
132  << url << ". Retrying in " << retry << "ms..." << endl;
133  usleep(1000 * retry);
134  continue;
135  }
136  return response;
137  }
139  }
140 
141  // Wrapper around cpr::Deletee requests which does retries in case of 429 response
142  cpr::Response cprDeleteWithRetry(cpr::Url url, cpr::Timeout timeout, cpr::Body body = cpr::Body{}) {
143 
144  // we need different headers if body is empty or not
145  cpr::Header header;
146  if (body == cpr::Body{}) {
147  header = cpr::Header{ { "accept", "application/json" }};
148  } else {
149  header = cpr::Header{ { "accept", "application/json" }, { "Content-Type", "application/json" }};
150  }
151 
152  for (int retry : wait_before_retry) {
153  auto response = cpr::Delete(url, timeout, body, header);
154  if (response.status_code==429) {
155  cout << "WARNING: Got http code 429 (too many requests) on "
156  << url << ". Retrying in " << retry << "ms..." << endl;
157  usleep(1000 * retry);
158  continue;
159  }
160  return response;
161  }
163  }
164 
165 }
166 
173 {
174 public:
175  static shared_ptr<TrackedDataReceiver> create(const string& ip_address, unsigned int& port, const string& stream,
176  shared_ptr<RemoteInterface> creator)
177  {
178  return shared_ptr<TrackedDataReceiver>(new TrackedDataReceiver(ip_address, port, stream, creator));
179  }
180 
182  {
183  try
184  {
185  creator_->deleteDestinationFromStream(stream_, dest_);
186  }
187  catch (exception& e)
188  {
189  cerr << "[TrackedDataReceiver] Could not remove my destination " << dest_ << " for stream type " << stream_
190  << " from rc_visard: " << e.what() << endl;
191  }
192  }
193 
194 protected:
195  TrackedDataReceiver(const string& ip_address, unsigned int& port, const string& stream,
196  shared_ptr<RemoteInterface> creator)
197  : DataReceiver(ip_address, port), dest_(ip_address + ":" + to_string(port)), stream_(stream), creator_(creator)
198  {
199  }
200 
201  string dest_, stream_;
202  shared_ptr<RemoteInterface> creator_;
203 };
204 
205 // map to store already created RemoteInterface objects
206 map<string, RemoteInterface::Ptr> RemoteInterface::remote_interfaces_ = map<string, RemoteInterface::Ptr>();
207 
208 RemoteInterface::Ptr RemoteInterface::create(const string& rc_visard_inet_addrs, unsigned int requests_timeout)
209 {
210  // check if interface is already opened
211  auto found = RemoteInterface::remote_interfaces_.find(rc_visard_inet_addrs);
212  if (found != RemoteInterface::remote_interfaces_.end())
213  {
214  return found->second;
215  }
216 
217  // if not, create it
218  auto new_remote_interface = Ptr(new RemoteInterface(rc_visard_inet_addrs, requests_timeout));
219  RemoteInterface::remote_interfaces_[rc_visard_inet_addrs] = new_remote_interface;
220 
221  return new_remote_interface;
222 }
223 
224 RemoteInterface::RemoteInterface(const string& rc_visard_ip, unsigned int requests_timeout)
225  : visard_addrs_(rc_visard_ip), initialized_(false), visard_version_(0.0),
226  base_url_("http://" + visard_addrs_ + "/api/v1"), timeout_curl_(requests_timeout)
227 {
228  req_streams_.clear();
229  protobuf_map_.clear();
230 
231  // check if given string is a valid IP address
232  if (!isValidIPAddress(rc_visard_ip))
233  {
234  throw invalid_argument("Given IP address is not a valid address: " + rc_visard_ip);
235  }
236 }
237 
239 {
240  try {
242  } catch (exception& e) {
243  cerr << "[RemoteInterface::~RemoteInterface] Could not clean up all previously requested streams: "
244  << e.what() << endl;
245  }
246  for (const auto& s : req_streams_)
247  {
248  if (s.second.size() > 0)
249  {
250  cerr << "[RemoteInterface::~RemoteInterface] Could not stop all previously requested"
251  " streams of type "
252  << s.first << " on rc_visard. Please check "
253  "device manually"
254  " ("
255  << base_url_ << "/datastreams/" << s.first << ")"
256  " for not containing any of the following legacy streams and"
257  " delete them otherwise, e.g. using the swagger UI ("
258  << "http://" + visard_addrs_ + "/api/swagger/)"
259  << ": " << toString(s.second) << endl;
260  }
261  }
262 }
263 
265 {
266  initialized_=false;
267  visard_version_=0.0;
268  req_streams_.clear();
269  protobuf_map_.clear();
270  avail_streams_.clear();
271 
272  cpr::Url url = cpr::Url{ base_url_ + "/system"};
273  auto response = cprGetWithRetry(url, cpr::Timeout{ timeout_curl_ });
274  if (response.status_code == 502) // bad gateway
275  {
276  return false;
277  }
278  handleCPRResponse(response);
279 
280  // initial connection to rc_visard to check if system is ready ...
281  auto get_system = cprGetWithRetry(cpr::Url{ base_url_ + "/system" },
282  cpr::Timeout{ timeout_curl_ });
283  handleCPRResponse(get_system);
284  auto j = json::parse(get_system.text);
285  if (!j["ready"])
286  {
287  return false;
288  }
289 
290  // ... and to get version of rc_visard
291  string version = j["firmware"]["active_image"]["image_version"];
292  std::smatch match;
293  if (std::regex_search(version, match, std::regex("v(\\d+).(\\d+).(\\d+)")))
294  {
295  visard_version_ = stof(match[0].str().substr(1,3));
296  }
297 
298  // ...and to get streams
299  auto get_streams = cprGetWithRetry(cpr::Url{ base_url_ + "/datastreams" },
300  cpr::Timeout{ timeout_curl_ });
301  handleCPRResponse(get_streams);
302 
303  // parse text of response into json object
304  j = json::parse(get_streams.text);
305  for (const auto& stream : j)
306  {
307  avail_streams_.push_back(stream["name"]);
308  protobuf_map_[stream["name"]] = stream["protobuf"];
309  }
310 
311  // return true if system is ready
312  initialized_ = true;
313  return true;
314 }
315 
316 string RemoteInterface::getState(const std::string& node) {
317  cpr::Url url = cpr::Url{ base_url_ + "/nodes/" + node + "/status"};
318  auto response = cprGetWithRetry(url, cpr::Timeout{ timeout_curl_ });
319  handleCPRResponse(response);
320  try
321  {
322  auto j = json::parse(response.text);
323  return j["values"]["state"];
324  } catch (std::domain_error &e) {
325  // in case "state" field does not exist, return UNKNOWN
326  return State::UNKNOWN;
327  }
328 }
329 
331 {
332  return getState("rc_dynamics");
333 }
334 
336 {
337  return getState("rc_slam");
338 }
339 
341 {
342  return getState("rc_stereo_ins");
343 }
344 
345 std::string RemoteInterface::callDynamicsService(std::string service_name)
346 {
347  cpr::Url url = cpr::Url{ base_url_ + "/nodes/rc_dynamics/services/" + service_name };
348  auto response = cprPutWithRetry(url, cpr::Timeout{ timeout_curl_ });
349  handleCPRResponse(response);
350  auto j = json::parse(response.text);
351  std::string entered_state;
352  bool accepted = true;
353 
354  try
355  {
356  entered_state = j["response"]["current_state"].get<std::string>();
357  if (entered_state != State::IDLE and entered_state != State::RUNNING and entered_state != State::FATAL and
358  entered_state != State::WAITING_FOR_INS and entered_state != State::WAITING_FOR_INS_AND_SLAM and
359  entered_state != State::WAITING_FOR_SLAM and entered_state != State::RUNNING_WITH_SLAM)
360  {
361  // mismatch between rc_dynamics states and states used in this class?
362  throw InvalidState(entered_state);
363  }
364 
365  accepted = j["response"]["accepted"].get<bool>();
366  }
367  catch (std::logic_error&)
368  {
369  // Maybe old interface version? If so just return the numeric code
370  // as string - it isn't used by the tools using the old interface
371  try
372  {
373  entered_state = std::to_string(j["response"]["enteredState"].get<int>());
374  }
375  catch (std::logic_error&)
376  {
377  // Real problem (may even be unrelated to parsing json. Let the user see what the response is.
378  cerr << "Logic error when parsing the response of a service call to rc_dynamics!\n";
379  cerr << "Service called: " << url << "\n";
380  cerr << "Response:"
381  << "\n";
382  cerr << response.text << "\n";
383  throw;
384  }
385  }
386 
387  if (!accepted)
388  {
389  throw NotAccepted(service_name);
390  }
391 
392  return entered_state;
393 }
394 
396 {
397  return callDynamicsService("restart");
398 }
400 {
401  return callDynamicsService("restart_slam");
402 }
404 {
405  return callDynamicsService("start");
406 }
408 {
409  return callDynamicsService("start_slam");
410 }
412 {
413  return callDynamicsService("stop");
414 }
416 {
417  return callDynamicsService("stop_slam");
418 }
419 
421 {
422  std::string service_name = "reset";
423  cpr::Url url = cpr::Url{ base_url_ + "/nodes/rc_slam/services/" + service_name };
424  auto response = cprPutWithRetry(url, cpr::Timeout{ timeout_curl_ });
425  handleCPRResponse(response);
426  auto j = json::parse(response.text);
427  std::string entered_state;
428  bool accepted = true;
429 
430  try
431  {
432  entered_state = j["response"]["current_state"].get<std::string>();
433  std::vector<std::string> valid_states = { "IDLE", "RUNNING", "FATAL", "WAITING_FOR_DATA",
434  "RESTARTING", "RESETTING", "HALTED" };
435  if (std::count(valid_states.begin(), valid_states.end(), entered_state) == 0)
436  {
437  // mismatch between rc_slam states and states used in this class?
438  throw InvalidState(entered_state);
439  }
440 
441  accepted = j["response"]["accepted"].get<bool>();
442  }
443  catch (std::logic_error& json_exception)
444  {
445  // Maybe old interface version? If so just return the numeric code
446  // as string - it isn't used by the tools using the old interface
447  try
448  {
449  entered_state = std::to_string(j["response"]["enteredState"].get<int>());
450  }
451  catch (std::logic_error& json_exception)
452  {
453  // Real problem (may even be unrelated to parsing json. Let the user see what the response is.
454  cerr << "Logic error when parsing the response of a service call to rc_dynamics!\n";
455  cerr << "Service called: " << url << "\n";
456  cerr << "Response:"
457  << "\n";
458  cerr << response.text << "\n";
459  throw;
460  }
461  }
462 
463  if (!accepted)
464  {
465  throw NotAccepted(service_name);
466  }
467 
468  return entered_state;
469 }
470 
471 RemoteInterface::ReturnCode RemoteInterface::callSlamService(std::string service_name, unsigned int timeout_ms)
472 {
473  cpr::Url url = cpr::Url{ base_url_ + "/nodes/rc_slam/services/" + service_name };
474  auto response = cprPutWithRetry(url, cpr::Timeout{ (int32_t)timeout_ms });
475  handleCPRResponse(response);
476  auto j = json::parse(response.text);
477 
478  ReturnCode return_code;
479 
480  try
481  {
482  return_code.value = j["response"]["return_code"]["value"].get<int>();
483  return_code.message = j["response"]["return_code"]["message"];
484  }
485  catch (std::logic_error& json_exception)
486  {
487  // Real problem (may even be unrelated to parsing json. Let the user see what the response is.
488  cerr << "Logic error when parsing the response of a service call to rc_dynamics!\n";
489  cerr << "Service called: " << url << "\n";
490  cerr << "Response:"
491  << "\n";
492  cerr << response.text << "\n";
493  throw;
494  }
495 
496  return return_code;
497 }
498 
500 {
501  return callSlamService("save_map", timeout_ms);
502 }
504 {
505  return callSlamService("load_map", timeout_ms);
506 }
508 {
509  return callSlamService("remove_map", timeout_ms);
510 }
511 
513 {
514  if (!initialized_ && !checkSystemReady())
515  {
516  throw std::runtime_error("RemoteInterface not properly initialized or rc_visard is not ready. "
517  "Please initialize with method RemoteInterface::checkSystemReady()!");
518  }
519  return avail_streams_;
520 }
521 
522 string RemoteInterface::getPbMsgTypeOfStream(const string& stream)
523 {
524  checkStreamTypeAvailable(stream);
525  return protobuf_map_[stream];
526 }
527 
528 list<string> RemoteInterface::getDestinationsOfStream(const string& stream)
529 {
530  checkStreamTypeAvailable(stream);
531 
532  list<string> destinations;
533 
534  // do get request on respective url (no parameters needed for this simple service call)
535  cpr::Url url = cpr::Url{ base_url_ + "/datastreams/" + stream };
536  auto get = cprGetWithRetry(url, cpr::Timeout{ timeout_curl_ });
537  handleCPRResponse(get);
538 
539  // parse result as json
540  auto j = json::parse(get.text);
541  for (auto dest : j["destinations"])
542  {
543  destinations.push_back(dest.get<string>());
544  }
545  return destinations;
546 }
547 
548 void RemoteInterface::addDestinationToStream(const string& stream, const string& destination)
549 {
550  checkStreamTypeAvailable(stream);
551 
552  // do put request on respective url
553  json js_args;
554  js_args["destination"] = json::array();
555  js_args["destination"].push_back(destination);
556  cpr::Url url = cpr::Url{ base_url_ + "/datastreams/" + stream };
557  auto put = cprPutWithRetry(url, cpr::Timeout{ timeout_curl_ }, cpr::Body{ js_args.dump() });
558  if (put.status_code == 403)
559  {
560  throw TooManyStreamDestinations(json::parse(put.text)["message"].get<string>());
561  }
562  handleCPRResponse(put);
563 
564  // keep track of added destinations
565  req_streams_[stream].push_back(destination);
566 }
567 
568 void RemoteInterface::deleteDestinationFromStream(const string& stream, const string& destination)
569 {
570  checkStreamTypeAvailable(stream);
571 
572  // do delete request on respective url
573  json js_args;
574  js_args["destination"] = json::array();
575  js_args["destination"].push_back(destination);
576  cpr::Url url = cpr::Url{ base_url_ + "/datastreams/" + stream };
577  auto del = cprDeleteWithRetry(url, cpr::Timeout{ timeout_curl_ }, cpr::Body{ js_args.dump() });
578  handleCPRResponse(del);
579 
580  // delete destination also from list of requested streams
581  auto& destinations = req_streams_[stream];
582  auto found = find(destinations.begin(), destinations.end(), destination);
583  if (found != destinations.end())
584  destinations.erase(found);
585 }
586 
587 void RemoteInterface::deleteDestinationsFromStream(const string& stream, const list<string>& destinations)
588 {
589  checkStreamTypeAvailable(stream);
590 
591  // with newer image versions this is the most efficent way, i.e. only one call
592  if (visard_version_ >= 1.600001) {
593 
594  // do delete request on respective url; list of destinationas are given as body
595  json js_destinations = json::array();
596  for (const auto& dest: destinations)
597  {
598  js_destinations.push_back(dest);
599  }
600  json js_args;
601  js_args["destination"] = js_destinations;
602  cpr::Url url = cpr::Url{ base_url_ + "/datastreams/" + stream };
603  auto del = cprDeleteWithRetry(url, cpr::Timeout{ timeout_curl_ }, cpr::Body{ js_args.dump()});
604  handleCPRResponse(del);
605 
606  // with older image versions we have to work around and do several calls
607  } else {
608  for (const auto& dest : destinations)
609  {
610  // do delete request on respective url
611  json js_args;
612  js_args["destination"] = json::array();
613  js_args["destination"].push_back(dest);
614  cpr::Url url = cpr::Url{ base_url_ + "/datastreams/" + stream };
615  auto del = cprDeleteWithRetry(url, cpr::Timeout{ timeout_curl_ }, cpr::Body{ js_args.dump() });
616  handleCPRResponse(del);
617  }
618  }
619 
620  // delete destination also from list of requested streams
621  auto& reqDestinations = req_streams_[stream];
622  for (auto& destination : destinations)
623  {
624  auto found = find(reqDestinations.begin(), reqDestinations.end(), destination);
625  if (found != reqDestinations.end())
626  {
627  reqDestinations.erase(found);
628  }
629  }
630 }
631 
632 namespace
633 {
634 roboception::msgs::Trajectory toProtobufTrajectory(const json js)
635 {
636  // TODO: find an automatic way to parse Messages from Json
637  // * is possible with protobuf >= 3.0.x
638  // * https://developers.google.com/protocol-buffers/docs/reference/cpp/google.protobuf.util.json_util
639  roboception::msgs::Trajectory pb_traj;
640 
641  json::const_iterator js_it;
642  if ((js_it = js.find("parent")) != js.end())
643  {
644  pb_traj.set_parent(js_it.value());
645  }
646  if ((js_it = js.find("name")) != js.end())
647  {
648  pb_traj.set_name(js_it.value());
649  }
650  if ((js_it = js.find("producer")) != js.end())
651  {
652  pb_traj.set_producer(js_it.value());
653  }
654  if ((js_it = js.find("timestamp")) != js.end())
655  {
656  pb_traj.mutable_timestamp()->set_sec(js_it.value()["sec"]); // TODO: sec
657  pb_traj.mutable_timestamp()->set_nsec(js_it.value()["nsec"]); // TODO: nsec
658  }
659  for (const auto& js_pose : js["poses"])
660  {
661  auto pb_pose = pb_traj.add_poses();
662  auto pb_time = pb_pose->mutable_timestamp();
663  pb_time->set_sec(js_pose["timestamp"]["sec"]); // TODO: sec
664  pb_time->set_nsec(js_pose["timestamp"]["nsec"]); // TODO: nsec
665  auto pb_position = pb_pose->mutable_pose()->mutable_position();
666  pb_position->set_x(js_pose["pose"]["position"]["x"]);
667  pb_position->set_y(js_pose["pose"]["position"]["y"]);
668  pb_position->set_z(js_pose["pose"]["position"]["z"]);
669  auto pb_orientation = pb_pose->mutable_pose()->mutable_orientation();
670  pb_orientation->set_x(js_pose["pose"]["orientation"]["x"]);
671  pb_orientation->set_y(js_pose["pose"]["orientation"]["y"]);
672  pb_orientation->set_z(js_pose["pose"]["orientation"]["z"]);
673  pb_orientation->set_w(js_pose["pose"]["orientation"]["w"]);
674  }
675  return pb_traj;
676 }
677 }
678 
679 roboception::msgs::Trajectory RemoteInterface::getSlamTrajectory(const TrajectoryTime& start, const TrajectoryTime& end, unsigned int timeout_ms)
680 {
681  // convert time specification to json obj
682  json js_args, js_time, js_start_time, js_end_time;
683  js_start_time["sec"] = start.getSec();
684  js_start_time["nsec"] = start.getNsec();
685  js_end_time["sec"] = end.getSec();
686  js_end_time["nsec"] = end.getNsec();
687  js_args["args"]["start_time"] = js_start_time;
688  js_args["args"]["end_time"] = js_end_time;
689  if (start.isRelative())
690  js_args["args"]["start_time_relative"] = true;
691  if (end.isRelative())
692  js_args["args"]["end_time_relative"] = true;
693 
694  // get request on slam module
695  cpr::Url url = cpr::Url{ base_url_ + "/nodes/rc_slam/services/get_trajectory" };
696  auto get = cprPutWithRetry(url, cpr::Timeout{ (int32_t)timeout_ms }, cpr::Body{ js_args.dump() });
697  handleCPRResponse(get);
698 
699  auto js = json::parse(get.text)["response"]["trajectory"];
700  return toProtobufTrajectory(js);
701 }
702 
703 DataReceiver::Ptr RemoteInterface::createReceiverForStream(const string& stream, const string& dest_interface,
704  unsigned int dest_port)
705 {
706  checkStreamTypeAvailable(stream);
707 
708  // figure out local inet address for streaming
709  string dest_address;
710  if (!getThisHostsIP(dest_address, visard_addrs_, dest_interface))
711  {
712  stringstream msg;
713  msg << "Could not infer a valid IP address "
714  "for this host as the destination of the stream! "
715  "Given network interface specification was '"
716  << dest_interface << "'.";
717  throw invalid_argument(msg.str());
718  }
719 
720  // create data receiver with port as specified
721  DataReceiver::Ptr receiver = TrackedDataReceiver::create(dest_address, dest_port, stream, shared_from_this());
722 
723  // do REST-API call requesting a UDP stream from rc_visard device
724  string destination = dest_address + ":" + to_string(dest_port);
725  addDestinationToStream(stream, destination);
726 
727  // waiting for first message; we set a long timeout for receiving data
728  unsigned int initial_timeOut = 5000;
729  receiver->setTimeout(initial_timeOut);
730  if (!receiver->receive(protobuf_map_[stream]))
731  {
732  // we did not receive any message; check why, e.g. dynamics not in correct state?
733  string current_state = getDynamicsState();
734  std::vector<std::string> valid_states = { "RUNNING", "RUNNING_WITH_SLAM" };
735  if (std::count(valid_states.begin(), valid_states.end(), current_state) == 0)
736  {
737  throw DynamicsNotRunning(current_state);
738  }
739 
740  // in other cases we cannot tell, what's the reason
741  throw UnexpectedReceiveTimeout(initial_timeOut);
742  }
743 
744  // stream established, prepare everything for normal pose receiving
745  receiver->setTimeout(100);
746  return receiver;
747 }
748 
750 {
751  // for each stream type stop all previously requested streams
752  for (auto const& s : req_streams_)
753  {
754  if (!s.second.empty())
755  {
756  deleteDestinationsFromStream(s.first, s.second);
757  }
758  }
759 }
760 
762 {
763  if (!initialized_ && !checkSystemReady())
764  {
765  throw std::runtime_error("RemoteInterface not properly initialized or rc_visard is not ready. "
766  "Please initialize with method RemoteInterface::checkSystemReady()!");
767  }
768  auto found = find(avail_streams_.begin(), avail_streams_.end(), stream);
769  if (found == avail_streams_.end())
770  {
771  stringstream msg;
772  msg << "Stream of type '" << stream << "' is not available on rc_visard " << visard_addrs_;
773  throw invalid_argument(msg.str());
774  }
775 }
776 }
777 }
Simple remote interface to access the dynamic state estimates of an rc_visard device as data streams...
std::string getState(const std::string &node)
std::map< std::string, std::list< std::string > > req_streams_
roboception::msgs::Trajectory getSlamTrajectory(const TrajectoryTime &start=TrajectoryTime::RelativeToStart(), const TrajectoryTime &end=TrajectoryTime::RelativeToEnd(), unsigned int timeout_ms=0)
Returns the Slam trajectory from the sensor.
std::string stop()
Stops rc_dynamics module.
std::shared_ptr< RemoteInterface > Ptr
std::string getPbMsgTypeOfStream(const std::string &stream)
Returns the name of the protobuf message class that corresponds to a given data stream and is require...
float visard_version_
rc_visard&#39;s firmware version as double, i.e. major.minor, e.g. 1.6
void checkStreamTypeAvailable(const std::string &stream)
void deleteDestinationsFromStream(const std::string &stream, const std::list< std::string > &destinations)
Deletes given destinations from a stream, i.e.
long getNsec() const
std::shared_ptr< DataReceiver > Ptr
Definition: data_receiver.h:72
std::list< std::string > getDestinationsOfStream(const std::string &stream)
Returns a list of all destinations registered to the specified rc_dynamics stream.
long getSec() const
Thrown if rc_dynamics is requested to receive dynamics data but component is not running.
TrackedDataReceiver(const string &ip_address, unsigned int &port, const string &stream, shared_ptr< RemoteInterface > creator)
std::string resetSlam()
Resets the SLAM module The Stereo INS will keep running, if it is.
std::string getStereoInsState()
Returns the current state of rc_stereo_ins module.
static const std::string RUNNING_WITH_SLAM
Stereo INS and SLAM are running.
DataReceiver::Ptr createReceiverForStream(const std::string &stream, const std::string &dest_interface="", unsigned int dest_port=0)
Convenience method that automatically.
bool initialized_
indicates if remote_interface was initialized properly at least once, see checkSystemReady() ...
static const std::string WAITING_FOR_INS_AND_SLAM
Waiting for IMU data, will proceed to WAITING_FOR_SLAM.
Thrown if a service call is not accepted.
std::list< std::string > getAvailableStreams()
Returns a list all available streams on rc_visard.
bool getThisHostsIP(string &this_hosts_ip, const string &other_hosts_ip, const string &network_interface)
Definition: net_utils.cc:236
bool checkSystemReady()
Connects with rc_visard and checks the system state of the rc_visard device.
std::string restart()
Restarts the rc_dynamics module to Stereo INS only mode.
static const std::string FATAL
An error has occured. May be resolvable by stopping.
static shared_ptr< TrackedDataReceiver > create(const string &ip_address, unsigned int &port, const string &stream, shared_ptr< RemoteInterface > creator)
void deleteDestinationFromStream(const std::string &stream, const std::string &destination)
Deletes a destination from a stream, i.e.
A simple receiver object for handling data streamed by rc_visard&#39;s rc_dynamics module.
Definition: data_receiver.h:69
bool isValidIPAddress(const std::string &ip)
Checks if given string is a valid IP address.
Definition: net_utils.cc:279
ReturnCode removeSlamMap(unsigned int timeout_ms=0)
Removes the SLAM map on the sensor.
static const std::string WAITING_FOR_INS
Waiting for IMU data, will proceed to RUNNING.
Thrown if the current_state response of the dynamics service does not correspond to those in the Stat...
std::string restartSlam()
Restarts the rc_dynamics module to SLAM mode.
Exception handling cases where actually everything should be fine and rc_visard&#39;s dynamic state estim...
void addDestinationToStream(const std::string &stream, const std::string &destination)
Adds a destination to a stream, i.e.
std::string stopSlam()
Stops only the SLAM module (via the rc_dynamics module).
shared_ptr< RemoteInterface > creator_
Thrown if too many streams are running already on rc_visard.
std::string getSlamState()
Returns the current state of rc_slam module.
Thrown if a REST API call is rejected because of too many requests.
ReturnCode callSlamService(std::string service_name, unsigned int timeout_ms=0)
call slam services which have a return code with value and message
ReturnCode saveSlamMap(unsigned int timeout_ms=0)
Saves the SLAM map on the sensor.
ReturnCode loadSlamMap(unsigned int timeout_ms=0)
Loads the SLAM map on the sensor.
std::string getDynamicsState()
Returns the current state of rc_dynamics module.
static const std::string IDLE
Not yet started or stopped.
int value
suceess >= 0, failure < 0
std::string callDynamicsService(std::string service_name)
Common functionality for start(), startSlam(), stop(), ...
static const std::string UNKNOWN
State of component is unknown, e.g. not yet reported.
nlohmann::json json
string toString(cpr::Response resp)
void handleCPRResponse(cpr::Response r)
std::string startSlam()
Sets rc_dynamics module to running state.
static const std::string RUNNING
Stereo INS is running.
bool isRelative() const
std::list< std::string > avail_streams_
static const std::string WAITING_FOR_SLAM
Stereo INS is running, waiting for SLAM data, will proceed to RUNNING_WITH_SLAM.
std::map< std::string, std::string > protobuf_map_
Represents a time stamp to query the trajectory of rcvisard&#39;s slam module.
string toString(list< string > list)
std::string start()
Sets rc_dynamics module to running state.
Class for data stream receivers that are created by this remote interface in order to keep track of c...


rc_dynamics_api
Author(s): Heiko Hirschmueller , Christian Emmerich , Felix Endres
autogenerated on Thu May 9 2019 02:51:36