int loadSettings()
{
int res = 0;
char iniFilePath[MAX_FILE_NAME_LEN];
// Load the configuration data
XPLMGetSystemPath(iniFilePath);
strcat(iniFilePath, "Resources\\plugins\\" PLUGIN_NAME "\\" PLUGIN_NAME ".ini");
// Read data from ini file
CIniReader configReader(iniFilePath);
logMessageEx("--- Configuration data loaded");
g_pos_x = configReader.ReadInteger(INI_SECTION_POSITION, "x", 0);
g_pos_y = configReader.ReadInteger(INI_SECTION_POSITION, "y", 0);
g_zoom = configReader.ReadFloat(INI_SECTION_POSITION, "zoom", 100);
logMessageEx("--- x=%d", g_pos_x);
logMessageEx("--- y=%d", g_pos_y);
logMessageEx("--- zoom=%f", g_zoom);
return res;
}
// Read the configuration file
// return: -1 failed; 1 succeed
int configureReader::ReadConfigFile( string fileName )
{
ifstream configReader( fileName.c_str() );
if( configReader == NULL )
{
cout<<"error reading configuration file"<<endl;
return -1;
}
string line;
while( getline( configReader, line ) )
{
if( line.size() > 0 && line.at( 0 ) != '#' )
{
// analyze the parameters
if( line == "[LIB]" )
continue;
if( AnalyzeParameters( line ) == -1 )
return -1;
}
}
return 1;
}
// TODO Change api of set_IP4_address to support const string.
void RTPSAsSocketReader::init(std::string &ip, uint32_t port, uint16_t nmsgs)
{
RTPSParticipantAttributes pattr;
pattr.builtin.use_SIMPLE_RTPSParticipantDiscoveryProtocol = false;
pattr.builtin.use_WriterLivelinessProtocol = false;
pattr.builtin.domainId = (uint32_t)GET_PID() % 230;
pattr.participantID = 1;
participant_ = RTPSDomain::createParticipant(pattr);
//Create readerhistory
HistoryAttributes hattr;
hattr.payloadMaxSize = 255;
history_ = new ReaderHistory(hattr);
//Create reader
ReaderAttributes rattr;
Locator_t loc;
loc.set_IP4_address(ip);
loc.port = port;
rattr.endpoint.multicastLocatorList.push_back(loc);
configReader(rattr);
reader_ = RTPSDomain::createRTPSReader(participant_, rattr, history_, &listener_);
//Add remote writer (in this case a reader in the same machine)
GUID_t guid = participant_->getGuid();
addRemoteWriter(reader_, ip, port, guid);
// Initialize list of msgs
for(uint16_t count = 0; count < nmsgs; ++count)
{
msgs_.push_back(count);
}
text_ = getText();
domainId_ = (uint32_t)GET_PID();
hostname_ = asio::ip::host_name();
std::ostringstream word;
word << text_ << "_" << hostname_ << "_" << domainId_;
word_ = word.str();
initialized_ = true;
}
// TODO Change api of set_IP4_address to support const string.
void RTPSAsSocketReader::init(std::string &ip, uint32_t port, uint16_t nmsgs)
{
RTPSParticipantAttributes pattr;
pattr.builtin.use_SIMPLE_RTPSParticipantDiscoveryProtocol = false;
pattr.builtin.use_WriterLivelinessProtocol = false;
pattr.participantID = 1;
participant_ = RTPSDomain::createParticipant(pattr);
ASSERT_NE(participant_, nullptr);
//Create readerhistory
HistoryAttributes hattr;
hattr.payloadMaxSize = 255;
history_ = new ReaderHistory(hattr);
ASSERT_NE(history_, nullptr);
//Create reader
ReaderAttributes rattr;
Locator_t loc;
loc.set_IP4_address(ip);
loc.port = port;
rattr.endpoint.multicastLocatorList.push_back(loc);
configReader(rattr);
reader_ = RTPSDomain::createRTPSReader(participant_, rattr, history_, &listener_);
ASSERT_NE(reader_, nullptr);
//Add remote writer (in this case a reader in the same machine)
GUID_t guid = participant_->getGuid();
addRemoteWriter(reader_, ip, port, guid);
// Initialize list of msgs
for(uint16_t count = 0; count < nmsgs; ++count)
{
msgs_.push_back(count);
}
initialized_ = true;
}
IIterativeClosestPointPtr IterativeClosestPointFactory::createIterativeClosestPoint(std::string configurationFile) {
if (((configurationFile.compare("")) == 0) || (configurationFile.compare("default") == 0)) {
return createIterativeClosestPoint();
}
ConfigurationFileHandler configReader(configurationFile);
if (configReader.getErrorsOccured()) {
cout << "WARNING: Errors during parsing configuration file. Factory fill provide default configuration." << endl;
return createIterativeClosestPoint();
}
std::stringstream summary;
summary << "#################### ICP Configuration ####################" << endl << "#" <<endl;
/* set up icp */
IIterativeClosestPointPtr icp; //abstract interface to ICP
// IIterativeClosestPointSetupPtr icpConfigurator; //setup interface
string icpImplementation;
configReader.getAttribute("IterativeClosestPoint", "implementation", &icpImplementation);
if (icpImplementation.compare("IterativeClosestPoint") == 0) {
IPointCorrespondence* assigner;
IRigidTransformationEstimation* estimator;
string subalgorithm;
summary << "# Implementation: IterativeClosestPoint" << endl;
/* process assigner */
if(configReader.getSubAlgorithm("IterativeClosestPoint", "PointCorrespondence", &subalgorithm)) {
if (subalgorithm.compare("PointCorrespondenceKDTree") == 0) {
assigner = new PointCorrespondenceKDTree();
summary << "# Subalgorithm: " << subalgorithm << endl;
// } else if (...) { //add more implemetation here
} else {
cout << "WARNING: PointCorrespondence implementation not found. Factory fill provide default configuration: PointCorrespondenceKDTree" << endl;
assigner = new PointCorrespondenceKDTree();
summary << "# Subalgorithm: PointCorrespondenceKDTree (DEFAULT)" << endl;
}
} else {
cout << "WARNING: PointCorrespondence implementation not found. Factory fill provide default configuration: PointCorrespondenceKDTree" << endl;
assigner = new PointCorrespondenceKDTree();
summary << "# Subalgorithm: PointCorrespondenceKDTree (DEFAULT)" << endl;
}
/* process estimator */
if(configReader.getSubAlgorithm("IterativeClosestPoint", "RigidTransformationEstimation", &subalgorithm)) {
if (subalgorithm.compare("RigidTransformationEstimationSVD") == 0) {
estimator = new RigidTransformationEstimationSVD();
summary << "# Subalgorithm: " << subalgorithm << endl;
} else if (subalgorithm.compare("RigidTransformationEstimationAPX") == 0) {
estimator = new RigidTransformationEstimationAPX();
summary << "# Subalgorithm: " << subalgorithm << endl;
} else if (subalgorithm.compare("RigidTransformationEstimationHELIX") == 0) {
estimator = new RigidTransformationEstimationHELIX();
summary << "# Subalgorithm: " << subalgorithm << endl;
} else if (subalgorithm.compare("RigidTransformationEstimationQUAT") == 0) {
estimator = new RigidTransformationEstimationQUAT();
summary << "# Subalgorithm: " << subalgorithm << endl;
} else if (subalgorithm.compare("RigidTransformationEstimationORTHO") == 0) {
estimator = new RigidTransformationEstimationORTHO();
summary << "# Subalgorithm: " << subalgorithm << endl;
// } else if (...) { //add more implementation here
} else {
cout << "WARNING: RigidTransformationEstimation implementation not found. Factory fill provide default configuration: RigidTransformationEstimationSVD" << endl;
estimator = new RigidTransformationEstimationSVD();
summary << "# Subalgorithm: RigidTransformationEstimationSVD (DEFAULT)" << endl;
}
} else {
cout << "WARNING: RigidTransformationEstimation implementation not found. Factory fill provide default configuration: RigidTransformationEstimationSVD" << endl;
estimator = new RigidTransformationEstimationSVD();
summary << "# Subalgorithm: RigidTransformationEstimationSVD (DEFAULT)" << endl;
}
icp = IIterativeClosestPointPtr(new IterativeClosestPoint(assigner, estimator));
boost::shared_ptr<IterativeClosestPoint> icpTmpHandle = boost::dynamic_pointer_cast<IterativeClosestPoint>(icp); //downcast
icpConfigurator = boost::dynamic_pointer_cast<IIterativeClosestPointSetup>(icp); //upcast to setup interface
} else { //Neither IterativeClosestPoint6DSLAM nor IterativeClosestPoint
cout << "WARNING: Errors during parsing configuration file. Factory fill provide default configuration." << endl;
return createIterativeClosestPoint();
}
/*
* process parameters of icp
*/
// IIterativeClosestPointSetup* icpConfigurator;
// std::tr1::shared_ptr<Basesp0(new Derived); // http://bytes.com/topic/c/answers/542252-boost-shared_ptr-polymorphism
// std::tr1::shared_ptr<Derivedsp1 = dynamic_pointer_cast<Derived>(sp0);
// boost::shared_ptr<Derived> dp=boost::dynamic_pointer_cast<Derived>(bp);
assert(icpConfigurator != 0); //cast worked => icp implementation implements IIterativeClosestPointSetup interface
int maxIterations;
if (configReader.getAttribute("IterativeClosestPoint", "maxIterations", &maxIterations)) {
icpConfigurator->setMaxIterations(maxIterations);
}
double convergenceThreshold;
if (configReader.getAttribute("IterativeClosestPoint", "convergenceThreshold", &convergenceThreshold)) {
icpConfigurator->setConvergenceThreshold(convergenceThreshold);
}
summary << "#" << endl << "# Parameters:" << endl;
summary << "# maxIterations = " << icpConfigurator->getMaxIterations() << endl;
summary << "# convergenceThreshold = " << icpConfigurator->getConvergenceThreshold() << endl;
summary << "#" << endl << "###########################################################" << endl;
LOG(INFO) << "Summary: " << std::endl << summary.str();
return icp;
}
