void IRUS::setup() { // Load scenario. const URL urlOfSCNXFile(m_kvc.getValue<string>("global.scenario")); if (urlOfSCNXFile.isValid()) { SCNXArchive &scnxArchive = SCNXArchiveFactory::getInstance().getSCNXArchive(urlOfSCNXFile); hesperia::data::scenario::Scenario &scenario = scnxArchive.getScenario(); const hesperia::data::scenario::Surroundings &surroundings = scenario.getGround().getSurroundings(); const vector<hesperia::data::scenario::Shape*> &listOfShapes = surroundings.getListOfShapes(); vector<hesperia::data::scenario::Shape*>::const_iterator it = listOfShapes.begin(); while (it != listOfShapes.end()) { hesperia::data::scenario::Shape *shape = (*it++); if (shape != NULL) { if (shape->getType() == hesperia::data::scenario::Shape::POLYGON) { hesperia::data::scenario::Polygon *polygon = dynamic_cast<hesperia::data::scenario::Polygon*>(shape); if (polygon != NULL) { Polygon p; m_numberOfPolygons++; const vector<hesperia::data::scenario::Vertex3> &listOfVertices = polygon->getListOfVertices(); vector<hesperia::data::scenario::Vertex3>::const_iterator jt = listOfVertices.begin(); while (jt != listOfVertices.end()) { p.add(*jt++); } m_mapOfPolygons[m_numberOfPolygons] = p; cerr << "[IRUS] Found polygon: " << p.toString() << endl; } } } } } // Setup all point sensors. for (uint32_t i = 0; i < m_kvc.getValue<uint32_t>("odsimirus.numberOfSensors"); i++) { stringstream sensorID; sensorID << "odsimirus.sensor" << i << ".id"; uint16_t id(m_kvc.getValue<uint16_t>(sensorID.str())); stringstream sensorName; sensorName << "odsimirus.sensor" << i << ".name"; string name(m_kvc.getValue<string>(sensorName.str())); stringstream sensorTranslation; sensorTranslation << "odsimirus.sensor" << i << ".translation"; Point3 translation(m_kvc.getValue<string>(sensorTranslation.str())); stringstream sensorRotZ; sensorRotZ << "odsimirus.sensor" << i << ".rotZ"; const double rotZ = m_kvc.getValue<double>(sensorRotZ.str()); stringstream sensorAngleFOV; sensorAngleFOV << "odsimirus.sensor" << i << ".angleFOV"; const double angleFOV = m_kvc.getValue<double>(sensorAngleFOV.str()); stringstream sensorDistanceFOV; sensorDistanceFOV << "odsimirus.sensor" << i << ".distanceFOV"; const double distanceFOV = m_kvc.getValue<double>(sensorDistanceFOV.str()); stringstream sensorClampDistance; sensorClampDistance << "odsimirus.sensor" << i << ".clampDistance"; const double clampDistance = m_kvc.getValue<double>(sensorClampDistance.str()); stringstream sensorShowFOV; sensorShowFOV << "odsimirus.sensor" << i << ".showFOV"; const bool showFOV = m_kvc.getValue<bool>(sensorShowFOV.str()); // Don't skip any values as default. double faultModelSkip = 0; try { stringstream faultModelSkipStr; faultModelSkipStr << "odsimirus.sensor" << i << ".faultModel.skip"; faultModelSkip = m_kvc.getValue<double>(faultModelSkipStr.str()); if (faultModelSkip < 0) { faultModelSkip = 0; } if (faultModelSkip > 1) { faultModelSkip = 1; } } catch (const core::exceptions::ValueForKeyNotFoundException &e) { } // Don't add any noise as default. double faultModelNoise = 0; try { stringstream faultModelNoiseStr; faultModelNoiseStr << "odsimirus.sensor" << i << ".faultModel.noise"; faultModelNoise = m_kvc.getValue<double>(faultModelNoiseStr.str()); } catch (const core::exceptions::ValueForKeyNotFoundException &e) { } PointSensor *ps = new PointSensor(id, name, translation, rotZ, angleFOV, distanceFOV, clampDistance, showFOV, faultModelSkip, faultModelNoise); if (ps != NULL) { // Save for later. m_mapOfPointSensors[ps->getName()] = ps; // Initialize distance map entry. m_distances[ps->getName()] = -1; // Initialize FOV map entry. Polygon f; m_FOVs[ps->getName()] = f; cout << "[IRUS] Registered point sensor " << ps->toString() << "." << endl; } } }
ModuleState::MODULE_EXITCODE IRUS::body() { // Load scenario. const URL urlOfSCNXFile(getKeyValueConfiguration().getValue<string>("global.scenario")); if (urlOfSCNXFile.isValid()) { SCNXArchive &scnxArchive = SCNXArchiveFactory::getInstance().getSCNXArchive(urlOfSCNXFile); hesperia::data::scenario::Scenario &scenario = scnxArchive.getScenario(); const hesperia::data::scenario::Surroundings &surroundings = scenario.getGround().getSurroundings(); const vector<hesperia::data::scenario::Shape*> &listOfShapes = surroundings.getListOfShapes(); vector<hesperia::data::scenario::Shape*>::const_iterator it = listOfShapes.begin(); while (it != listOfShapes.end()) { hesperia::data::scenario::Shape *shape = (*it++); if (shape != NULL) { if (shape->getType() == hesperia::data::scenario::Shape::POLYGON) { hesperia::data::scenario::Polygon *polygon = dynamic_cast<hesperia::data::scenario::Polygon*>(shape); if (polygon != NULL) { Polygon p; m_numberOfPolygons++; const vector<hesperia::data::scenario::Vertex3> &listOfVertices = polygon->getListOfVertices(); vector<hesperia::data::scenario::Vertex3>::const_iterator jt = listOfVertices.begin(); while (jt != listOfVertices.end()) { p.add(*jt++); } m_mapOfPolygons[m_numberOfPolygons] = p; } } } } } // Show found polygons on console and in monitor. const bool showPolygons = getKeyValueConfiguration().getValue<bool>("irus.showPolygons"); if (showPolygons) { map<uint32_t, Polygon>::iterator it = m_mapOfPolygons.begin(); while (it != m_mapOfPolygons.end()) { const uint32_t polygonID = it->first; Polygon p = it->second; Obstacle polygonObstacle(polygonID, Obstacle::UPDATE); polygonObstacle.setPolygon(p); // Send obstacle. Container c = Container(Container::OBSTACLE, polygonObstacle); getConference().send(c); cerr << "Found polygon: " << it->second.toString() << endl; it++; } } // Setup all point sensors. for (uint32_t i = 0; i < getKeyValueConfiguration().getValue<uint32_t>("irus.numberOfSensors"); i++) { stringstream sensorID; sensorID << "irus.sensor" << i << ".id"; uint16_t id(getKeyValueConfiguration().getValue<uint16_t>(sensorID.str())); stringstream sensorName; sensorName << "irus.sensor" << i << ".name"; string name(getKeyValueConfiguration().getValue<string>(sensorName.str())); stringstream sensorTranslation; sensorTranslation << "irus.sensor" << i << ".translation"; Point3 translation(getKeyValueConfiguration().getValue<string>(sensorTranslation.str())); stringstream sensorRotZ; sensorRotZ << "irus.sensor" << i << ".rotZ"; const double rotZ = getKeyValueConfiguration().getValue<double>(sensorRotZ.str()); stringstream sensorAngleFOV; sensorAngleFOV << "irus.sensor" << i << ".angleFOV"; const double angleFOV = getKeyValueConfiguration().getValue<double>(sensorAngleFOV.str()); stringstream sensorDistanceFOV; sensorDistanceFOV << "irus.sensor" << i << ".distanceFOV"; const double distanceFOV = getKeyValueConfiguration().getValue<double>(sensorDistanceFOV.str()); stringstream sensorClampDistance; sensorClampDistance << "irus.sensor" << i << ".clampDistance"; const double clampDistance = getKeyValueConfiguration().getValue<double>(sensorClampDistance.str()); stringstream sensorShowFOV; sensorShowFOV << "irus.sensor" << i << ".showFOV"; const bool showFOV = getKeyValueConfiguration().getValue<bool>(sensorShowFOV.str()); PointSensor *ps = new PointSensor(id, name, translation, rotZ, angleFOV, distanceFOV, clampDistance, showFOV); if (ps != NULL) { // Save for later. m_mapOfPointSensors[ps->getName()] = ps; // Initialize distance map entry. m_distances[ps->getName()] = -1; // Initialize FOV map entry. Polygon f; m_FOVs[ps->getName()] = f; cout << "Registered point sensor " << ps->toString() << "." << endl; } } // Use the most recent EgoState available. KeyValueDataStore &kvs = getKeyValueDataStore(); // MSV: Variable for data from the sensorboard. msv::SensorBoardData sensorBoardData; // Loop through the map of polygons with the current EgoState and intersect all with the point sensor's FOV. while (getModuleState() == ModuleState::RUNNING) { // Get current EgoState. Container c = kvs.get(Container::EGOSTATE); EgoState es = c.getData<EgoState>(); // Loop through point sensors. map<string, PointSensor*, core::wrapper::StringComparator>::iterator sensorIterator = m_mapOfPointSensors.begin(); for (; sensorIterator != m_mapOfPointSensors.end(); sensorIterator++) { PointSensor *sensor = sensorIterator->second; // Update FOV. Polygon FOV = sensor->updateFOV(es.getPosition(), es.getRotation()); m_FOVs[sensor->getName()] = FOV; // Calculate distance. m_distances[sensor->getName()] = sensor->getDistance(m_mapOfPolygons); cerr << sensor->getName() << ": " << m_distances[sensor->getName()] << endl; // MSV: Store data for sensorboard. sensorBoardData.update(sensor->getID(), m_distances[sensor->getName()]); // MSV: Create a container with type USER_DATA_0. c = Container(Container::USER_DATA_0, sensorBoardData); // MSV: Send container. getConference().send(c); } // Distribute FOV where necessary. uint32_t sensorID = 9000; map<string, Polygon, core::wrapper::StringComparator>::iterator FOVIterator = m_FOVs.begin(); for (; FOVIterator != m_FOVs.end(); FOVIterator++) { string key = FOVIterator->first; Polygon FOV = FOVIterator->second; PointSensor *ps = m_mapOfPointSensors[key]; if ( (ps != NULL) && (ps->hasShowFOV()) ) { // Send FOV. Obstacle FOVobstacle(sensorID++, Obstacle::UPDATE); FOVobstacle.setPolygon(FOV); // Send obstacle. c = Container(Container::OBSTACLE, FOVobstacle); getConference().send(c); } } } // Delete all point sensors. map<string, PointSensor*, core::wrapper::StringComparator>::const_iterator sensorIterator = m_mapOfPointSensors.begin(); for (; sensorIterator != m_mapOfPointSensors.end(); sensorIterator++) { PointSensor *sensor = sensorIterator->second; OPENDAVINCI_CORE_DELETE_POINTER(sensor); } m_mapOfPointSensors.clear(); return ModuleState::OKAY; }