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";
63 s <<
"status code: " << resp.status_code << endl
64 <<
"url: " << resp.url << endl
65 <<
"text: " << resp.text << endl
66 <<
"error: " << resp.error.message;
74 for (
auto it = list.begin(); it != list.end();)
77 if (++it != list.end())
88 switch (r.status_code) {
100 vector<int> wait_before_retry = { 50, 200, 500, 1000, 2000};
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);
118 cpr::Response cprPutWithRetry(cpr::Url url, cpr::Timeout timeout, cpr::Body body = cpr::Body{}) {
122 if (body == cpr::Body{}) {
123 header = cpr::Header{ {
"accept",
"application/json" }};
125 header = cpr::Header{ {
"accept",
"application/json" }, {
"Content-Type",
"application/json" }};
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);
142 cpr::Response cprDeleteWithRetry(cpr::Url url, cpr::Timeout timeout, cpr::Body body = cpr::Body{}) {
146 if (body == cpr::Body{}) {
147 header = cpr::Header{ {
"accept",
"application/json" }};
149 header = cpr::Header{ {
"accept",
"application/json" }, {
"Content-Type",
"application/json" }};
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);
175 static shared_ptr<TrackedDataReceiver>
create(
const string& ip_address,
unsigned int& port,
const string& stream,
176 shared_ptr<RemoteInterface> creator)
178 return shared_ptr<TrackedDataReceiver>(
new TrackedDataReceiver(ip_address, port, stream, creator));
185 creator_->deleteDestinationFromStream(stream_, dest_);
189 cerr <<
"[TrackedDataReceiver] Could not remove my destination " << dest_ <<
" for stream type " << stream_
190 <<
" from rc_visard: " << e.what() << endl;
196 shared_ptr<RemoteInterface> creator)
197 :
DataReceiver(ip_address, port), dest_(ip_address +
":" + to_string(port)), stream_(stream), creator_(creator)
206 map<string, RemoteInterface::Ptr> RemoteInterface::remote_interfaces_ = map<string, RemoteInterface::Ptr>();
208 RemoteInterface::Ptr RemoteInterface::create(
const string& rc_visard_inet_addrs,
unsigned int requests_timeout)
211 auto found = RemoteInterface::remote_interfaces_.find(rc_visard_inet_addrs);
212 if (found != RemoteInterface::remote_interfaces_.end())
214 return found->second;
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;
221 return new_remote_interface;
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)
234 throw invalid_argument(
"Given IP address is not a valid address: " + rc_visard_ip);
242 }
catch (exception& e) {
243 cerr <<
"[RemoteInterface::~RemoteInterface] Could not clean up all previously requested streams: " 248 if (s.second.size() > 0)
250 cerr <<
"[RemoteInterface::~RemoteInterface] Could not stop all previously requested" 252 << s.first <<
" on rc_visard. Please check " 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 (" 259 <<
": " <<
toString(s.second) << endl;
272 cpr::Url url = cpr::Url{
base_url_ +
"/system"};
273 auto response = cprGetWithRetry(url, cpr::Timeout{
timeout_curl_ });
274 if (response.status_code == 502)
281 auto get_system = cprGetWithRetry(cpr::Url{
base_url_ +
"/system" },
284 auto j = json::parse(get_system.text);
291 string version = j[
"firmware"][
"active_image"][
"image_version"];
293 if (std::regex_search(version, match, std::regex(
"v(\\d+).(\\d+).(\\d+)")))
299 auto get_streams = cprGetWithRetry(cpr::Url{
base_url_ +
"/datastreams" },
304 j = json::parse(get_streams.text);
305 for (
const auto& stream : j)
317 cpr::Url url = cpr::Url{
base_url_ +
"/nodes/" + node +
"/status"};
318 auto response = cprGetWithRetry(url, cpr::Timeout{
timeout_curl_ });
322 auto j = json::parse(response.text);
323 return j[
"values"][
"state"];
324 }
catch (std::domain_error &e) {
347 cpr::Url url = cpr::Url{
base_url_ +
"/nodes/rc_dynamics/services/" + service_name };
348 auto response = cprPutWithRetry(url, cpr::Timeout{
timeout_curl_ });
350 auto j = json::parse(response.text);
351 std::string entered_state;
352 bool accepted =
true;
356 entered_state = j[
"response"][
"current_state"].get<std::string>();
365 accepted = j[
"response"][
"accepted"].get<
bool>();
367 catch (std::logic_error&)
373 entered_state = std::to_string(j[
"response"][
"enteredState"].get<int>());
375 catch (std::logic_error&)
378 cerr <<
"Logic error when parsing the response of a service call to rc_dynamics!\n";
379 cerr <<
"Service called: " << url <<
"\n";
382 cerr << response.text <<
"\n";
392 return entered_state;
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_ });
426 auto j = json::parse(response.text);
427 std::string entered_state;
428 bool accepted =
true;
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)
441 accepted = j[
"response"][
"accepted"].get<
bool>();
443 catch (std::logic_error& json_exception)
449 entered_state = std::to_string(j[
"response"][
"enteredState"].get<int>());
451 catch (std::logic_error& json_exception)
454 cerr <<
"Logic error when parsing the response of a service call to rc_dynamics!\n";
455 cerr <<
"Service called: " << url <<
"\n";
458 cerr << response.text <<
"\n";
468 return entered_state;
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 });
476 auto j = json::parse(response.text);
482 return_code.
value = j[
"response"][
"return_code"][
"value"].get<
int>();
483 return_code.
message = j[
"response"][
"return_code"][
"message"];
485 catch (std::logic_error& json_exception)
488 cerr <<
"Logic error when parsing the response of a service call to rc_dynamics!\n";
489 cerr <<
"Service called: " << url <<
"\n";
492 cerr << response.text <<
"\n";
516 throw std::runtime_error(
"RemoteInterface not properly initialized or rc_visard is not ready. " 517 "Please initialize with method RemoteInterface::checkSystemReady()!");
532 list<string> destinations;
535 cpr::Url url = cpr::Url{
base_url_ +
"/datastreams/" + stream };
536 auto get = cprGetWithRetry(url, cpr::Timeout{
timeout_curl_ });
540 auto j = json::parse(
get.text);
541 for (
auto dest : j[
"destinations"])
543 destinations.push_back(dest.get<
string>());
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)
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() });
582 auto found = find(destinations.begin(), destinations.end(), destination);
583 if (found != destinations.end())
584 destinations.erase(found);
595 json js_destinations = json::array();
596 for (
const auto& dest: destinations)
598 js_destinations.push_back(dest);
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()});
608 for (
const auto& dest : destinations)
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() });
622 for (
auto& destination : destinations)
624 auto found = find(reqDestinations.begin(), reqDestinations.end(), destination);
625 if (found != reqDestinations.end())
627 reqDestinations.erase(found);
634 roboception::msgs::Trajectory toProtobufTrajectory(
const json js)
639 roboception::msgs::Trajectory pb_traj;
641 json::const_iterator js_it;
642 if ((js_it = js.find(
"parent")) != js.end())
644 pb_traj.set_parent(js_it.value());
646 if ((js_it = js.find(
"name")) != js.end())
648 pb_traj.set_name(js_it.value());
650 if ((js_it = js.find(
"producer")) != js.end())
652 pb_traj.set_producer(js_it.value());
654 if ((js_it = js.find(
"timestamp")) != js.end())
656 pb_traj.mutable_timestamp()->set_sec(js_it.value()[
"sec"]);
657 pb_traj.mutable_timestamp()->set_nsec(js_it.value()[
"nsec"]);
659 for (
const auto& js_pose : js[
"poses"])
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"]);
664 pb_time->set_nsec(js_pose[
"timestamp"][
"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"]);
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;
690 js_args[
"args"][
"start_time_relative"] =
true;
692 js_args[
"args"][
"end_time_relative"] =
true;
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() });
699 auto js = json::parse(
get.text)[
"response"][
"trajectory"];
700 return toProtobufTrajectory(js);
704 unsigned int dest_port)
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());
724 string destination = dest_address +
":" + to_string(dest_port);
728 unsigned int initial_timeOut = 5000;
729 receiver->setTimeout(initial_timeOut);
734 std::vector<std::string> valid_states = {
"RUNNING",
"RUNNING_WITH_SLAM" };
735 if (std::count(valid_states.begin(), valid_states.end(), current_state) == 0)
745 receiver->setTimeout(100);
754 if (!s.second.empty())
765 throw std::runtime_error(
"RemoteInterface not properly initialized or rc_visard is not ready. " 766 "Please initialize with method RemoteInterface::checkSystemReady()!");
772 msg <<
"Stream of type '" << stream <<
"' is not available on rc_visard " <<
visard_addrs_;
773 throw invalid_argument(msg.str());
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'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.
std::shared_ptr< DataReceiver > Ptr
std::list< std::string > getDestinationsOfStream(const std::string &stream)
Returns a list of all destinations registered to the specified rc_dynamics stream.
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)
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's rc_dynamics module.
bool isValidIPAddress(const std::string &ip)
Checks if given string is a valid IP address.
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.
std::string visard_addrs_
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.
virtual ~TrackedDataReceiver()
void cleanUpRequestedStreams()
Exception handling cases where actually everything should be fine and rc_visard's dynamic state estim...
void addDestinationToStream(const std::string &stream, const std::string &destination)
Adds a destination to a stream, i.e.
virtual ~RemoteInterface()
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.
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.
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'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...