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 {
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 };
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
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
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
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
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
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 }