Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include <fstream>
00037 #include <signal.h>
00038 #include <chrono>
00039 #include <iomanip>
00040
00041 #include "rc_dynamics_api/remote_interface.h"
00042 #include "csv_printing.h"
00043
00044 using namespace std;
00045 using namespace rc::dynamics;
00046
00047
00051 static bool caught_signal = false;
00052 void signal_callback_handler(int signum)
00053 {
00054 printf("Caught signal %d, stopping program!\n",signum);
00055 caught_signal = true;
00056 }
00057
00058
00062 void printUsage(char *arg)
00063 {
00064 cout << "\nLists available rcdynamics data streams of the specified rc_visard IP, "
00065 "\nor requests a data stream and either prints received messages or records "
00066 "\nthem as csv-file, see -o option."
00067 << "\n\nUsage: \n\t"
00068 << arg
00069 << " -v rcVisardIP -l | -s stream [-i networkInterface]"
00070 "\n\t\t[-n maxNumData][-t maxRecTimeSecs][-o outputFile]"
00071 << endl;
00072 }
00073
00074
00075 int main(int argc, char *argv[])
00076 {
00077
00078 signal(SIGINT, signal_callback_handler);
00079 signal(SIGTERM, signal_callback_handler);
00080
00081
00085 string outputFileName, visardIP, networkIface = "", streamName;
00086 unsigned int maxNumRecordingMsgs = 50, maxRecordingTimeSecs = 5;
00087 bool userSetOutputFile = false;
00088 bool userSetMaxNumMsgs = false;
00089 bool userSetRecordingTime = false;
00090 bool userSetIp = false;
00091 bool userSetStreamType = false;
00092 bool onlyListStreams = false;
00093
00094 int opt;
00095 while ((opt = getopt(argc, argv, "hln:v:i:o:t:s:")) != -1)
00096 {
00097 switch (opt)
00098 {
00099 case 'l':
00100 onlyListStreams = true;
00101 break;
00102 case 's':
00103 streamName = string(optarg);
00104 userSetStreamType = true;
00105 break;
00106 case 'i':
00107 networkIface = string(optarg);
00108 break;
00109 case 'v':
00110 visardIP = string(optarg);
00111 userSetIp = true;
00112 break;
00113 case 'n':
00114 maxNumRecordingMsgs = (unsigned int) max(0, atoi(optarg));
00115 userSetMaxNumMsgs = true;
00116 break;
00117 case 't':
00118 maxRecordingTimeSecs = (unsigned int) max(0, atoi(optarg));
00119 userSetRecordingTime = true;
00120 break;
00121 case 'o':
00122 outputFileName = string(optarg);
00123 userSetOutputFile = true;
00124 break;
00125 case 'h':
00126 printUsage(argv[0]);
00127 return EXIT_SUCCESS;
00128 default:
00129 printUsage(argv[0]);
00130 return EXIT_FAILURE;
00131 }
00132 }
00133 if (!userSetIp)
00134 {
00135 cerr << "Please specify rc_visard IP." << endl;
00136 printUsage(argv[0]);
00137 return EXIT_FAILURE;
00138 }
00139 if (!userSetStreamType && !onlyListStreams)
00140 {
00141 cerr << "Please specify stream type." << endl;
00142 printUsage(argv[0]);
00143 return EXIT_FAILURE;
00144 }
00145 if (!userSetMaxNumMsgs && !userSetRecordingTime)
00146 {
00147 userSetMaxNumMsgs = true;;
00148 }
00149
00153 ofstream outputFile;
00154 if (userSetOutputFile)
00155 {
00156 outputFile.open(outputFileName);
00157 if (!outputFile.is_open())
00158 {
00159 cerr << "Could not open file '" << outputFileName << "' for writing!"
00160 << endl;
00161 return EXIT_FAILURE;
00162 }
00163 }
00164
00168 cout << "connecting to rc_visard " << visardIP << "..." << endl;
00169 auto rcvisardDynamics = RemoteInterface::create(visardIP);
00170
00171
00172 if (onlyListStreams)
00173 {
00174 auto streams = rcvisardDynamics->getAvailableStreams();
00175 string firstColumn = "Available streams:";
00176 size_t firstColumnWidth = firstColumn.length();
00177 for (auto&& s : streams)
00178 if (s.length() > firstColumnWidth)
00179 firstColumnWidth = s.length();
00180 firstColumnWidth += 5;
00181 cout << left << setw(firstColumnWidth) << firstColumn << "Protobuf message types:" << endl;
00182 for (auto&& s : streams)
00183 cout << left << setw(firstColumnWidth) << s << rcvisardDynamics->getPbMsgTypeOfStream(s) << endl;
00184 cout << endl;
00185 return EXIT_SUCCESS;
00186 }
00187
00188
00189 if (streamName != "imu")
00190 {
00191 try
00192 {
00193
00194 cout << "starting rc_dynamics module on rc_visard..." << endl;
00195 rcvisardDynamics->start();
00196 }
00197 catch (exception &e)
00198 {
00199 cout << "ERROR! Could not start rc_dynamics module on rc_visard: "
00200 << e.what() << endl;
00201 return EXIT_FAILURE;
00202 }
00203 }
00204
00208 unsigned int cntMsgs = 0;
00209 try
00210 {
00211 cout << "Initializing " << streamName << " data stream..." << endl;
00212 auto receiver = rcvisardDynamics->createReceiverForStream(streamName, networkIface);
00213
00214 unsigned int timeoutMillis = 100;
00215 receiver->setTimeout(timeoutMillis);
00216 cout << "Listening for " << streamName << " messages..." << endl;
00217
00218 chrono::time_point<chrono::system_clock> start = chrono::system_clock::now();
00219 chrono::duration<double> elapsedSecs(0);
00220 while (!caught_signal
00221 && (!userSetMaxNumMsgs || cntMsgs < maxNumRecordingMsgs)
00222 && (!userSetRecordingTime ||
00223 elapsedSecs.count() < maxRecordingTimeSecs)
00224 )
00225 {
00226 auto msg = receiver->receive(rcvisardDynamics->getPbMsgTypeOfStream(streamName));
00227 if (msg)
00228 {
00229 if (outputFile.is_open())
00230 {
00231 if (cntMsgs==0)
00232 {
00233 csv::Header h;
00234 outputFile << (h << *msg) << endl;
00235 }
00236 csv::Line l;
00237 outputFile << (l << *msg) << endl;
00238 }
00239 else
00240 {
00241 cout << "received " << streamName << " msg:" << endl
00242 << msg->DebugString() << endl;
00243 }
00244 ++cntMsgs;
00245 }
00246 else
00247 {
00248 cerr << "did not receive any data during last " << timeoutMillis
00249 << " ms." << endl;
00250 }
00251 elapsedSecs = chrono::system_clock::now() - start;
00252 }
00253
00254 }
00255 catch (exception &e)
00256 {
00257 cout << "Caught exception during streaming, stopping: " << e.what() << endl;
00258 }
00259
00260
00265 if (streamName != "imu")
00266 {
00267 try
00268 {
00269 cout << "stopping rc_dynamics module on rc_visard..." << endl;
00270 rcvisardDynamics->stop();
00271 }
00272 catch (exception &e)
00273 {
00274 cout << "Caught exception: " << e.what() << endl;
00275 }
00276 }
00277
00278 if (outputFile.is_open())
00279 {
00280 outputFile.close();
00281 cout << "Recorded " << cntMsgs << " " << streamName << " messages to '"
00282 << outputFileName << "'." << endl;
00283 }
00284 else
00285 {
00286 cout << "Received " << cntMsgs << " " << streamName << " messages." << endl;
00287 }
00288
00289 return EXIT_SUCCESS;
00290 }