00001
00002
00003
00004
00005
00006
00007
00008
00009 #include "Whorl.h"
00010 #include "WhorlSim.h"
00011 #include "CfgParse.h"
00012 #include "Measurement.h"
00013 #include <iostream>
00014 #include <sstream>
00015
00016 #include "ace/ACE.h"
00017 #include "ace/Log_Msg.h"
00018 #include "Utils/SystemProperties.h"
00019 #include "Comm/CommFactory.h"
00020 #include "Comm/MessageClient.h"
00021 #include "Comm/MessageServer.h"
00022
00023 using namespace std;
00024
00025
00026
00027
00028
00029
00030
00031 class RG_Writer : public MessageClient
00032 {
00033 public:
00034
00035
00036
00037 enum { MY_MSG_ID = 101 };
00038
00039
00040
00041
00042
00043
00044 RG_Writer(MessageServer *pMsgSvr) : MessageClient(MY_MSG_ID)
00045 { Register(pMsgSvr); }
00046
00047
00048
00049
00050
00051 int SendMessage(const char *msg, u_int dest)
00052 {
00053 ACE_DEBUG((LM_DEBUG,
00054 "Sending this message to the reader: %s\n", msg));
00055
00056 return MessageClient::SendMessage(msg, strlen(msg)+1, dest);
00057 }
00058
00059
00060
00061
00062
00063
00064
00065
00066 int ReceiveMessage(const char *msg, size_t len)
00067 {
00068
00069 ACE_DEBUG((LM_DEBUG, "Writer got a message: %s\n", msg));
00070
00071 return 0;
00072 }
00073 };
00074
00075
00076
00077
00078
00079
00080
00081
00082 class RG_Reader : public MessageClient
00083 {
00084 public:
00085
00086
00087
00088 enum { MY_MSG_ID = 100 };
00089
00090
00091
00092
00093
00094
00095 RG_Reader(MessageServer *pMsgSvr) : MessageClient(MY_MSG_ID)
00096 { Register(pMsgSvr); }
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111 int ReceiveMessage(const char *msg, size_t len)
00112 {
00113 ACE_DEBUG((LM_DEBUG, "Reader got a message: %s\n", msg));
00114
00115 istringstream iss(msg);
00116
00117 double rg[3];
00118
00119 iss >> rg[0] >> rg[1] >> rg[2];
00120
00121 ACE_DEBUG((LM_DEBUG, " Rate gyro readings: %f %f %f\n",
00122 rg[0], rg[1], rg[2]));
00123
00124 ACE_DEBUG((LM_DEBUG, "Sending the reply message 'OK' to the writer.\n"));
00125
00126 this->SendMessage("OK", 3, RG_Writer::MY_MSG_ID);
00127
00128 return 0;
00129 }
00130 };
00131
00132
00133
00134 int main() {
00135
00136
00137
00138
00139 {
00140 u_long mask =
00141 ACE_Log_Msg::instance()->priority_mask(ACE_Log_Msg::PROCESS);
00142
00143 ACE_Log_Msg::instance()->priority_mask(mask & ~LM_INFO,
00144 ACE_Log_Msg::PROCESS);
00145 }
00146
00147
00148 cout << "Code is running." << endl;
00149
00150
00151
00152
00153 SystemProperties *props = SystemProperties::Instance("System.config");
00154
00155
00156
00157
00158
00159 PropertyBasedCommFactory commFact;
00160
00161
00162
00163
00164 if (commFact.Open(*props, "Typhon\\Comm") == -1)
00165 {
00166 ACE_ERROR_RETURN((LM_ERROR, "Comm setup failed.\n"), -1);
00167 }
00168
00169
00170
00171
00172 MessageServer *pMsgSvr = commFact.GetMessageServer();
00173
00174
00175
00176
00177 RG_Writer myWriter(pMsgSvr);
00178
00179
00180
00181
00182 RG_Reader myReader(pMsgSvr);
00183
00184
00185
00186 cfgBody *cfgDat;
00187 CfgParse parser;
00188
00189
00190 cout << "Parsing Whorl configuration file." << endl;
00191 cfgDat = parser.go("simpleComm.cfg");
00192
00193 Whorl whorlOne;
00194 whorlOne.Initialize(cfgDat);
00195
00196 timeval t;
00197 whorlOne.GetRateGyro(string("dmu_rg1"))->GetMeasurement().GetTime(t);
00198 whorlOne.SetTimeOfEstimate(t);
00199
00200
00201 Measurement rateGyroMeasurements[3];
00202 Vector rateGyroDoubles(3);
00203
00204 cout << "Entering the loop." << endl;
00205
00206 ACE_OS::sleep(5);
00207
00208 for ( int ii = 1; ii <= 100; ii++ ) {
00209
00210
00211
00212 rateGyroMeasurements[0] =
00213 whorlOne.GetRateGyro(string("dmu_rg1"))->GetMeasurement();
00214 rateGyroMeasurements[1] =
00215 whorlOne.GetRateGyro(string("dmu_rg2"))->GetMeasurement();
00216 rateGyroMeasurements[2] =
00217 whorlOne.GetRateGyro(string("dmu_rg3"))->GetMeasurement();
00218
00219
00220
00221
00222
00223 ostringstream oss;
00224 oss << rateGyroMeasurements[0].GetAsDouble() << " "
00225 << rateGyroMeasurements[1].GetAsDouble() << " "
00226 << rateGyroMeasurements[2].GetAsDouble();
00227
00228
00229
00230
00231 const char *msg = oss.str().c_str();
00232
00233
00234
00235 if (myWriter.SendMessage(msg, RG_Reader::MY_MSG_ID) == -1)
00236 {
00237 ACE_ERROR_RETURN((LM_ERROR,
00238 "***Sending a message failed. Aborting.\n"), -1);
00239 }
00240
00241 cout << " Sent." << endl;
00242
00243 };
00244
00245
00246 cout << "Out of the loop." << endl;
00247
00248
00249
00250 ACE_OS::sleep(3);
00251
00252
00253 return 0;
00254
00255 };
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281