ExternalCalUtility.cc
Go to the documentation of this file.
00001 
00037 #ifdef WIN32
00038 #ifndef WIN32_LEAN_AND_MEAN
00039 #define WIN32_LEAN_AND_MEAN 1
00040 #endif
00041 
00042 #include <windows.h>
00043 #include <winsock2.h>
00044 #else
00045 #include <unistd.h>
00046 #endif
00047 
00048 #include <stdio.h>
00049 #include <stdlib.h>
00050 #include <sys/types.h>
00051 #include <sys/stat.h>
00052 #include <fstream>
00053 #include <map>
00054 #include <string.h>
00055 
00056 #include <Utilities/portability/getopt/getopt.h>
00057 #include <Utilities/shared/CalibrationYaml.hh>
00058 #include <LibMultiSense/MultiSenseChannel.hh>
00059 
00060 using namespace crl::multisense;
00061 
00062 namespace {  // anonymous
00063 
00064 void usage(const char *programNameP) 
00065 {
00066     fprintf(stderr, 
00067             "USAGE: %s -f <calibration_file> [<options>]\n", 
00068             programNameP);
00069     fprintf(stderr, "Where <options> are:\n");
00070     fprintf(stderr, "\t-a <ip_address>      : ip address (default=10.66.171.21)\n");
00071     fprintf(stderr, "\t-s                   : set the external calibration (default is query)\n");
00072     fprintf(stderr, "\t-y                   : disable confirmation prompts\n");
00073     
00074     exit(-1);
00075 }
00076 
00077 bool fileExists(const std::string& name)
00078 {
00079     struct stat sbuf;
00080     return (0 == stat(name.c_str(), &sbuf));
00081 }
00082 
00083 const char *externalCalibrationNameP       = "external_calibration";
00084 
00085 
00086 std::ostream& writeCal (std::ostream& stream, system::ExternalCalibration const& calibration)
00087 {
00088     stream << "%YAML:1.0\n";
00089 
00090     float tmpCal[6];
00091     tmpCal[0] = calibration.x;
00092     tmpCal[1] = calibration.y;
00093     tmpCal[2] = calibration.z;
00094     tmpCal[3] = calibration.roll;
00095     tmpCal[4] = calibration.pitch;
00096     tmpCal[5] = calibration.yaw;
00097 
00098     writeMatrix (stream, externalCalibrationNameP, 1, 6, &tmpCal[0]);
00099     return stream;
00100 }
00101 
00102 
00103 }; // anonymous
00104 
00105 int main(int    argc, 
00106          char **argvPP)
00107 {
00108     std::string        ipAddress = "10.66.171.21";
00109     std::string        calFile;
00110     bool               setCal=false;
00111     bool               prompt=true;
00112 
00113     //
00114     // Parse args
00115 
00116     int c;
00117 
00118     while(-1 != (c = getopt(argc, argvPP, "a:f:sy")))
00119         switch(c) {
00120         case 'a': ipAddress = std::string(optarg);    break;
00121         case 'f': calFile   = std::string(optarg);    break;
00122         case 's': setCal    = true;                   break;
00123         case 'y': prompt    = false;                  break;
00124         default: usage(*argvPP);                      break;
00125         }
00126 
00127     //
00128     // Verify options
00129 
00130     if (calFile.empty()) {
00131         fprintf(stderr, "Must provide a file argument\n");
00132         usage(*argvPP);
00133     }
00134 
00135     if (true == setCal && false == fileExists(calFile)) {
00136         
00137         fprintf(stderr, "file not found: \"%s\"\n", calFile.c_str());
00138         usage(*argvPP);
00139     }
00140     
00141     if (false == setCal && true == prompt &&
00142         true == fileExists(calFile)) {
00143         
00144         fprintf(stdout, 
00145                 "\"%s\" already exists.\n\n"
00146                 "Really overwrite this file? (y/n): ",
00147                 calFile.c_str());
00148         fflush(stdout);
00149 
00150         int c = getchar();
00151         if ('Y' != c && 'y' != c) {
00152             fprintf(stdout, "Aborting\n");
00153             return 0;
00154         }
00155     }
00156 
00157     //
00158     // Initialize communications.
00159 
00160     Channel *channelP = Channel::Create(ipAddress);
00161     if (NULL == channelP) {
00162         fprintf(stderr, "Failed to establish communications with \"%s\"\n",
00163                 ipAddress.c_str());
00164         return -1;
00165     }
00166 
00167     //
00168     // Query version
00169 
00170     Status status;
00171     VersionType version;
00172 
00173     status = channelP->getSensorVersion(version);
00174     if (Status_Ok != status) {
00175         fprintf(stderr, "failed to query sensor version: %s\n", 
00176                 Channel::statusString(status));
00177         goto clean_out;
00178     }
00179     
00180     //
00181     // Query
00182 
00183     if (false == setCal) {
00184 
00185         system::ExternalCalibration c;
00186 
00187         status = channelP->getExternalCalibration(c);
00188         if (Status_Ok != status) {
00189             fprintf(stderr, "failed to query external calibration: %s\n", 
00190                     Channel::statusString(status));
00191             goto clean_out;
00192         }
00193 
00194         std::ofstream cvFile (calFile.c_str (), std::ios_base::out | std::ios_base::trunc);
00195 
00196         if (!cvFile) {
00197             fprintf(stderr, "failed to open '%s' for writing\n", 
00198                     calFile.c_str());
00199             goto clean_out;
00200         }
00201 
00202         writeCal (cvFile, c);
00203 
00204         cvFile.flush ();
00205 
00206     } else {
00207 
00208         std::map<std::string, std::vector<float> > data;
00209         std::ifstream cvFile (calFile.c_str ());
00210 
00211         if (!cvFile) {
00212             fprintf(stderr, "failed to open '%s' for reading\n", 
00213                     calFile.c_str());
00214             goto clean_out;
00215         }
00216 
00217         parseYaml (cvFile, data);
00218 
00219         cvFile.close ();
00220 
00221         if (data[externalCalibrationNameP].size () != 6 ) {
00222 
00223             fprintf(stderr, "calibration matrices incomplete in %s, "
00224                     "expecting two 4x4 matrices\n", calFile.c_str());
00225             goto clean_out;
00226         }
00227 
00228         system::ExternalCalibration c;
00229 
00230         c.x = data[externalCalibrationNameP][0];
00231         c.y = data[externalCalibrationNameP][1];
00232         c.z = data[externalCalibrationNameP][2];
00233         c.roll = data[externalCalibrationNameP][3];
00234         c.pitch = data[externalCalibrationNameP][4];
00235         c.yaw = data[externalCalibrationNameP][5];
00236 
00237         status = channelP->setExternalCalibration(c);
00238         if (Status_Ok != status) {
00239             fprintf(stderr, "failed to set external calibration: %s\n", 
00240                     Channel::statusString(status));
00241             goto clean_out;
00242         }
00243 
00244         fprintf(stdout, "External calibration successfully updated\n");
00245     }
00246 
00247 clean_out:
00248 
00249     Channel::Destroy(channelP);
00250     return 0;
00251 }


multisense_lib
Author(s):
autogenerated on Fri Apr 5 2019 02:28:24