vector<Container> IRUS::calculate(const EgoState &es) {
vector<Container> retVal;
// Store distance information.
automotive::miniature::SensorBoardData sensorBoardData;
// Loop through point sensors.
map<string, PointSensor*, core::strings::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;
// Store data for sensorboard.
sensorBoardData.putTo_MapOfDistances(sensor->getID(), m_distances[sensor->getName()]);
}
// Create a container with type USER_DATA_0.
Container c = Container(Container::USER_DATA_0, sensorBoardData);
// Enqueue container.
retVal.push_back(c);
// Distribute FOV where necessary.
uint32_t sensorID = 9000;
map<string, Polygon, core::strings::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);
Container c2 = Container(Container::OBSTACLE, FOVobstacle);
// Enqueue container.
retVal.push_back(c2);
}
}
return retVal;
}
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;
}
