51 const std::string RemoteInterface::State::IDLE =
"IDLE";
52 const std::string RemoteInterface::State::RUNNING =
"RUNNING";
53 const std::string RemoteInterface::State::STOPPING =
"STOPPING";
54 const std::string RemoteInterface::State::FATAL =
"FATAL";
55 const std::string RemoteInterface::State::WAITING_FOR_INS =
"WAITING_FOR_INS";
56 const std::string RemoteInterface::State::WAITING_FOR_INS_AND_SLAM =
"WAITING_FOR_INS_AND_SLAM";
57 const std::string RemoteInterface::State::WAITING_FOR_SLAM =
"WAITING_FOR_SLAM";
58 const std::string RemoteInterface::State::RUNNING_WITH_SLAM =
"RUNNING_WITH_SLAM";
59 const std::string RemoteInterface::State::UNKNOWN =
"UNKNOWN";
64 s <<
"status code: " << resp.status_code << endl
65 <<
"url: " << resp.url << endl
66 <<
"text: " << resp.text << endl
67 <<
"error: " << resp.error.message;
75 for (
auto it = list.begin(); it != list.end();)
78 if (++it != list.end())
89 switch (r.status_code) {
103 vector<int> wait_before_retry = { 50, 200, 500, 1000, 2000};
106 cpr::Response cprGetWithRetry(cpr::Url url, cpr::Timeout timeout) {
107 for (
int retry : wait_before_retry) {
108 auto response = cpr::Get(url, timeout, cpr::Header{ {
"accept",
"application/json" }});
109 if (response.status_code == 429) {
110 cout <<
"WARNING: Got http code 429 (too many requests) on " 111 << url <<
". Retrying in " << retry <<
"ms..." << endl;
115 usleep(1000 * retry);
125 cpr::Response cprPutWithRetry(cpr::Url url, cpr::Timeout timeout, cpr::Body body = cpr::Body{}) {
129 if (body == cpr::Body{}) {
130 header = cpr::Header{ {
"accept",
"application/json" }};
132 header = cpr::Header{ {
"accept",
"application/json" }, {
"Content-Type",
"application/json" }};
135 for (
int retry : wait_before_retry) {
136 auto response = cpr::Put(url, timeout, body, header);
137 if (response.status_code==429) {
138 cout <<
"WARNING: Got http code 429 (too many requests) on " 139 << url <<
". Retrying in " << retry <<
"ms..." << endl;
143 usleep(1000 * retry);
153 cpr::Response cprDeleteWithRetry(cpr::Url url, cpr::Timeout timeout, cpr::Body body = cpr::Body{}) {
157 if (body == cpr::Body{}) {
158 header = cpr::Header{ {
"accept",
"application/json" }};
160 header = cpr::Header{ {
"accept",
"application/json" }, {
"Content-Type",
"application/json" }};
163 for (
int retry : wait_before_retry) {
164 auto response = cpr::Delete(url, timeout, body, header);
165 if (response.status_code==429) {
166 cout <<
"WARNING: Got http code 429 (too many requests) on " 167 << url <<
". Retrying in " << retry <<
"ms..." << endl;
171 usleep(1000 * retry);
190 static shared_ptr<TrackedDataReceiver>
create(
const string& ip_address,
unsigned int& port,
const string& stream,
191 shared_ptr<RemoteInterface> creator)
193 return shared_ptr<TrackedDataReceiver>(
new TrackedDataReceiver(ip_address, port, stream, creator));
200 creator_->deleteDestinationFromStream(stream_, dest_);
204 cerr <<
"[TrackedDataReceiver] Could not remove my destination " << dest_ <<
" for stream type " << stream_
205 <<
" from rc_visard: " << e.what() << endl;
211 shared_ptr<RemoteInterface> creator)
212 :
DataReceiver(ip_address, port), dest_(ip_address +
":" + to_string(port)), stream_(stream), creator_(creator)
221 map<string, RemoteInterface::Ptr> RemoteInterface::remote_interfaces_ = map<string, RemoteInterface::Ptr>();
223 RemoteInterface::Ptr RemoteInterface::create(
const string& rc_visard_inet_addrs,
unsigned int requests_timeout)
226 auto found = RemoteInterface::remote_interfaces_.find(rc_visard_inet_addrs);
227 if (found != RemoteInterface::remote_interfaces_.end())
229 return found->second;
233 auto new_remote_interface =
Ptr(
new RemoteInterface(rc_visard_inet_addrs, requests_timeout));
234 RemoteInterface::remote_interfaces_[rc_visard_inet_addrs] = new_remote_interface;
236 return new_remote_interface;
239 RemoteInterface::RemoteInterface(
const string& rc_visard_ip,
unsigned int requests_timeout)
240 : visard_addrs_(rc_visard_ip), initialized_(false), visard_version_(0.0),
241 base_url_(
"http://" + visard_addrs_ +
"/api/v1"), timeout_curl_(requests_timeout)
249 throw invalid_argument(
"Given IP address is not a valid address: " + rc_visard_ip);
257 }
catch (exception& e) {
258 cerr <<
"[RemoteInterface::~RemoteInterface] Could not clean up all previously requested streams: " 263 if (s.second.size() > 0)
265 cerr <<
"[RemoteInterface::~RemoteInterface] Could not stop all previously requested" 267 << s.first <<
" on rc_visard. Please check " 270 <<
base_url_ <<
"/datastreams/" << s.first <<
")" 271 " for not containing any of the following legacy streams and" 272 " delete them otherwise, e.g. using the swagger UI (" 274 <<
": " <<
toString(s.second) << endl;
287 cpr::Url url = cpr::Url{
base_url_ +
"/system"};
288 auto response = cprGetWithRetry(url, cpr::Timeout{
timeout_curl_ });
289 if (response.status_code == 502)
296 auto get_system = cprGetWithRetry(cpr::Url{
base_url_ +
"/system" },
299 auto j = json::parse(get_system.text);
306 string version = j[
"firmware"][
"active_image"][
"image_version"];
308 if (std::regex_search(version, match, std::regex(
"v(\\d+).(\\d+).(\\d+)")))
314 auto get_streams = cprGetWithRetry(cpr::Url{
base_url_ +
"/datastreams" },
319 j = json::parse(get_streams.text);
320 for (
const auto& stream : j)
332 cpr::Url url = cpr::Url{
base_url_ +
"/nodes/" + node +
"/status"};
333 auto response = cprGetWithRetry(url, cpr::Timeout{
timeout_curl_ });
337 auto j = json::parse(response.text);
338 return j[
"values"][
"state"];
339 }
catch (std::domain_error &e) {
362 cpr::Url url = cpr::Url{
base_url_ +
"/nodes/rc_dynamics/services/" + service_name };
363 auto response = cprPutWithRetry(url, cpr::Timeout{
timeout_curl_ });
365 auto j = json::parse(response.text);
366 std::string entered_state;
367 bool accepted =
true;
371 const static vector<string> valid_states = {
382 entered_state = j[
"response"][
"current_state"].get<std::string>();
383 if (std::count(valid_states.begin(), valid_states.end(), entered_state) == 0)
389 accepted = j[
"response"][
"accepted"].get<
bool>();
391 catch (std::logic_error&)
397 entered_state = std::to_string(j[
"response"][
"enteredState"].get<int>());
399 catch (std::logic_error&)
402 cerr <<
"Logic error when parsing the response of a service call to rc_dynamics!\n";
403 cerr <<
"Service called: " << url <<
"\n";
406 cerr << response.text <<
"\n";
416 return entered_state;
446 std::string service_name =
"reset";
447 cpr::Url url = cpr::Url{
base_url_ +
"/nodes/rc_slam/services/" + service_name };
448 auto response = cprPutWithRetry(url, cpr::Timeout{
timeout_curl_ });
450 auto j = json::parse(response.text);
451 std::string entered_state;
452 bool accepted =
true;
456 entered_state = j[
"response"][
"current_state"].get<std::string>();
457 std::vector<std::string> valid_states = {
"IDLE",
"RUNNING",
"FATAL",
"WAITING_FOR_DATA",
458 "RESTARTING",
"RESETTING",
"HALTED" };
459 if (std::count(valid_states.begin(), valid_states.end(), entered_state) == 0)
465 accepted = j[
"response"][
"accepted"].get<
bool>();
467 catch (std::logic_error& json_exception)
473 entered_state = std::to_string(j[
"response"][
"enteredState"].get<int>());
475 catch (std::logic_error& json_exception)
478 cerr <<
"Logic error when parsing the response of a service call to rc_dynamics!\n";
479 cerr <<
"Service called: " << url <<
"\n";
482 cerr << response.text <<
"\n";
492 return entered_state;
497 cpr::Url url = cpr::Url{
base_url_ +
"/nodes/rc_slam/services/" + service_name };
498 auto response = cprPutWithRetry(url, cpr::Timeout{ (int32_t)timeout_ms });
500 auto j = json::parse(response.text);
506 return_code.
value = j[
"response"][
"return_code"][
"value"].get<
int>();
507 return_code.
message = j[
"response"][
"return_code"][
"message"];
509 catch (std::logic_error& json_exception)
512 cerr <<
"Logic error when parsing the response of a service call to rc_dynamics!\n";
513 cerr <<
"Service called: " << url <<
"\n";
516 cerr << response.text <<
"\n";
540 throw std::runtime_error(
"RemoteInterface not properly initialized or rc_visard is not ready. " 541 "Please initialize with method RemoteInterface::checkSystemReady()!");
556 list<string> destinations;
559 cpr::Url url = cpr::Url{
base_url_ +
"/datastreams/" + stream };
560 auto get = cprGetWithRetry(url, cpr::Timeout{
timeout_curl_ });
564 auto j = json::parse(
get.text);
565 for (
auto dest : j[
"destinations"])
567 destinations.push_back(dest.get<
string>());
578 js_args[
"destination"] = json::array();
579 js_args[
"destination"].push_back(destination);
580 cpr::Url url = cpr::Url{
base_url_ +
"/datastreams/" + stream };
581 auto put = cprPutWithRetry(url, cpr::Timeout{
timeout_curl_ }, cpr::Body{ js_args.dump() });
582 if (put.status_code == 403)
598 js_args[
"destination"] = json::array();
599 js_args[
"destination"].push_back(destination);
600 cpr::Url url = cpr::Url{
base_url_ +
"/datastreams/" + stream };
601 auto del = cprDeleteWithRetry(url, cpr::Timeout{
timeout_curl_ }, cpr::Body{ js_args.dump() });
606 auto found = find(destinations.begin(), destinations.end(), destination);
607 if (found != destinations.end())
608 destinations.erase(found);
619 json js_destinations = json::array();
620 for (
const auto& dest: destinations)
622 js_destinations.push_back(dest);
625 js_args[
"destination"] = js_destinations;
626 cpr::Url url = cpr::Url{
base_url_ +
"/datastreams/" + stream };
627 auto del = cprDeleteWithRetry(url, cpr::Timeout{
timeout_curl_ }, cpr::Body{ js_args.dump()});
632 for (
const auto& dest : destinations)
636 js_args[
"destination"] = json::array();
637 js_args[
"destination"].push_back(dest);
638 cpr::Url url = cpr::Url{
base_url_ +
"/datastreams/" + stream };
639 auto del = cprDeleteWithRetry(url, cpr::Timeout{
timeout_curl_ }, cpr::Body{ js_args.dump() });
646 for (
auto& destination : destinations)
648 auto found = find(reqDestinations.begin(), reqDestinations.end(), destination);
649 if (found != reqDestinations.end())
651 reqDestinations.erase(found);
663 roboception::msgs::Trajectory toProtobufTrajectory(
const json js)
665 roboception::msgs::Trajectory pb_traj;
667 json::const_iterator js_it;
668 if ((js_it = js.find(
"parent")) != js.end())
670 pb_traj.set_parent(js_it.value());
672 if ((js_it = js.find(
"name")) != js.end())
674 pb_traj.set_name(js_it.value());
676 if ((js_it = js.find(
"producer")) != js.end())
678 pb_traj.set_producer(js_it.value());
680 if ((js_it = js.find(
"timestamp")) != js.end())
682 pb_traj.mutable_timestamp()->set_sec(js_it.value()[
"sec"]);
683 pb_traj.mutable_timestamp()->set_nsec(js_it.value()[
"nsec"]);
685 for (
const auto& js_pose : js[
"poses"])
687 auto pb_pose = pb_traj.add_poses();
688 auto pb_time = pb_pose->mutable_timestamp();
689 pb_time->set_sec(js_pose[
"timestamp"][
"sec"]);
690 pb_time->set_nsec(js_pose[
"timestamp"][
"nsec"]);
691 auto pb_position = pb_pose->mutable_pose()->mutable_position();
692 pb_position->set_x(js_pose[
"pose"][
"position"][
"x"]);
693 pb_position->set_y(js_pose[
"pose"][
"position"][
"y"]);
694 pb_position->set_z(js_pose[
"pose"][
"position"][
"z"]);
695 auto pb_orientation = pb_pose->mutable_pose()->mutable_orientation();
696 pb_orientation->set_x(js_pose[
"pose"][
"orientation"][
"x"]);
697 pb_orientation->set_y(js_pose[
"pose"][
"orientation"][
"y"]);
698 pb_orientation->set_z(js_pose[
"pose"][
"orientation"][
"z"]);
699 pb_orientation->set_w(js_pose[
"pose"][
"orientation"][
"w"]);
704 roboception::msgs::Frame toProtobufFrame(
const json& js,
bool producer_optional)
706 roboception::msgs::Frame pb_frame;
708 pb_frame.set_parent(js.at(
"parent").get<
string>());
709 pb_frame.set_name(js.at(
"name").get<
string>());
712 if (!producer_optional || js.find(
"producer") != js.end())
714 pb_frame.set_producer(js.at(
"producer").get<
string>());
717 auto js_pose = js.at(
"pose");
718 auto pb_pose = pb_frame.mutable_pose();
719 pb_pose->mutable_timestamp()->set_sec(js_pose.at(
"timestamp").at(
"sec"));
720 pb_pose->mutable_timestamp()->set_nsec(js_pose.at(
"timestamp").at(
"nsec"));
722 auto js_pose_pose = js_pose.at(
"pose");
723 auto pb_pose_pose = pb_pose->mutable_pose();
724 pb_pose_pose->mutable_position()->set_x(js_pose_pose.at(
"position").at(
"x"));
725 pb_pose_pose->mutable_position()->set_y(js_pose_pose.at(
"position").at(
"y"));
726 pb_pose_pose->mutable_position()->set_z(js_pose_pose.at(
"position").at(
"z"));
727 pb_pose_pose->mutable_orientation()->set_w(js_pose_pose.at(
"orientation").at(
"w"));
728 pb_pose_pose->mutable_orientation()->set_x(js_pose_pose.at(
"orientation").at(
"x"));
729 pb_pose_pose->mutable_orientation()->set_y(js_pose_pose.at(
"orientation").at(
"y"));
730 pb_pose_pose->mutable_orientation()->set_z(js_pose_pose.at(
"orientation").at(
"z"));
740 json js_args, js_time, js_start_time, js_end_time;
741 js_start_time[
"sec"] = start.
getSec();
742 js_start_time[
"nsec"] = start.
getNsec();
743 js_end_time[
"sec"] = end.
getSec();
744 js_end_time[
"nsec"] = end.
getNsec();
745 js_args[
"args"][
"start_time"] = js_start_time;
746 js_args[
"args"][
"end_time"] = js_end_time;
748 js_args[
"args"][
"start_time_relative"] =
true;
750 js_args[
"args"][
"end_time_relative"] =
true;
753 cpr::Url url = cpr::Url{
base_url_ +
"/nodes/rc_slam/services/get_trajectory" };
754 auto get = cprPutWithRetry(url, cpr::Timeout{ (int32_t)timeout_ms }, cpr::Body{ js_args.dump() });
757 auto js = json::parse(
get.text)[
"response"][
"trajectory"];
758 return toProtobufTrajectory(js);
764 cpr::Url url = cpr::Url{
base_url_ +
"/nodes/rc_dynamics/services/get_cam2imu_transform" };
765 auto get = cprPutWithRetry(url, cpr::Timeout{ (int32_t)timeout_ms });
768 auto js = json::parse(
get.text)[
"response"];
769 return toProtobufFrame(js,
true);
773 unsigned int dest_port)
782 msg <<
"Could not infer a valid IP address " 783 "for this host as the destination of the stream! " 784 "Given network interface specification was '" 785 << dest_interface <<
"'.";
786 throw invalid_argument(msg.str());
793 string destination = dest_address +
":" + to_string(dest_port);
797 unsigned int initial_timeOut = 5000;
798 receiver->setTimeout(initial_timeOut);
803 std::vector<std::string> valid_states = {
"RUNNING",
"RUNNING_WITH_SLAM" };
804 if (std::count(valid_states.begin(), valid_states.end(), current_state) == 0)
814 receiver->setTimeout(100);
823 if (!s.second.empty())
834 throw std::runtime_error(
"RemoteInterface not properly initialized or rc_visard is not ready. " 835 "Please initialize with method RemoteInterface::checkSystemReady()!");
841 msg <<
"Stream of type '" << stream <<
"' is not available on rc_visard " <<
visard_addrs_;
842 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.
roboception::msgs::Frame getCam2ImuTransform(unsigned int timeout_ms=0)
Returns the transformation from camera to IMU coordinate frame.
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.
static const std::string STOPPING
Intermediate state while transitioning to IDLE (e.g. from 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.
Thrown if a REST API call is rejected because of 404; i.e. URL not found.
Class for data stream receivers that are created by this remote interface in order to keep track of c...