rcdynamics_stream.cc
Go to the documentation of this file.
00001 /*
00002  * This file is part of the rc_dynamics_api package.
00003  *
00004  * Copyright (c) 2017 Roboception GmbH
00005  * All rights reserved
00006  *
00007  * Author: Christian Emmerich
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  * 1. Redistributions of source code must retain the above copyright notice,
00013  * this list of conditions and the following disclaimer.
00014  *
00015  * 2. Redistributions in binary form must reproduce the above copyright notice,
00016  * this list of conditions and the following disclaimer in the documentation
00017  * and/or other materials provided with the distribution.
00018  *
00019  * 3. Neither the name of the copyright holder nor the names of its contributors
00020  * may be used to endorse or promote products derived from this software without
00021  * specific prior written permission.
00022  *
00023  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00024  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00025  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00026  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
00027  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00028  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00029  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00030  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00031  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00032  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  * POSSIBILITY OF SUCH DAMAGE.
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   // Register signals and signal handler
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   /* Only list available streams of device and exit */
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   /* For all streams except 'imu' the rc_dynamcis node has to be started */
00189   if (streamName != "imu")
00190   {
00191     try
00192     {
00193       // start the rc::dynamics module on the rc_visard
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 }


rc_visard_driver
Author(s): Heiko Hirschmueller , Christian Emmerich , Felix Ruess
autogenerated on Thu Jun 6 2019 20:43:06