void DataRenderer::draw(const EgoState &es) {
Lock l(m_dataRendererMutex);
if (m_renderer != NULL) {
if (m_egoStateModel.size() > 0) {
// Use model of triangle sets for drawing.
m_renderer->beginPainting();
// TODO: Rotation for OpenGL is given in Eulerian angles for X-axis, for Y-axis, and for Z-axis SEPERATELY!
Point3 dir(0, 0, es.getRotation().getAngleXY());
m_renderer->drawListOfTriangleSets(m_egoStateModel, es.getPosition(), dir);
m_renderer->endPainting();
}
else {
// Use box.
Point3 p1(2, 1, 0);
Point3 p2(2, -1, 0);
Point3 p3(-2, -1, 0);
Point3 p4(-2, 1, 0);
p1.rotateZ(es.getRotation().getAngleXY());
p1 += es.getPosition();
p2.rotateZ(es.getRotation().getAngleXY());
p2 += es.getPosition();
p3.rotateZ(es.getRotation().getAngleXY());
p3 += es.getPosition();
p4.rotateZ(es.getRotation().getAngleXY());
p4 += es.getPosition();
vector<Point3> listOfPoints;
listOfPoints.push_back(p1);
listOfPoints.push_back(p2);
listOfPoints.push_back(p3);
listOfPoints.push_back(p4);
listOfPoints.push_back(p1);
m_renderer->beginPainting();
m_renderer->setColor(Point3(0.7, 0.7, 0.7));
m_renderer->setLineWidth(2.0);
m_renderer->drawPolyLine(listOfPoints, 1.0);
m_renderer->endPainting();
}
}
}
vector<Container> IRUS::calculate(const EgoState &es) {
vector<Container> retVal;
// Store distance information.
automotive::miniature::SensorBoardData sensorBoardData;
// Loop through point sensors.
map<string, PointSensor*, odcore::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 automotive::miniature::SensorBoardData.
Container c = Container(sensorBoardData);
// Enqueue container.
retVal.push_back(c);
// Distribute FOV where necessary.
uint32_t sensorID = 9000;
map<string, Polygon, odcore::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(FOVobstacle);
// Enqueue container.
retVal.push_back(c2);
}
}
return retVal;
}
void Renderer::process(Container &c) {
m_drawMap[c.getDataType()] = c;
// Get data for following the EgoState.
if (c.getDataType() == Container::EGOSTATE) {
EgoState egostate = c.getData<EgoState>();
Point3 dirCamera(-10, 0, 0);
dirCamera.rotateZ(egostate.getRotation().getAngleXY());
m_positionCamera.setX(egostate.getPosition().getX() + dirCamera.getX());
m_positionCamera.setY(egostate.getPosition().getY() + dirCamera.getY());
m_positionCamera.setZ(10);
m_lookAtPointCamera.setX(egostate.getPosition().getX());
m_lookAtPointCamera.setY(egostate.getPosition().getY());
m_lookAtPointCamera.setZ(0);
}
}
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;
}
void EnvironmentViewerGLWidget::nextContainer(Container &c) {
if (c.getDataType() == Container::EGOSTATE) {
m_numberOfReceivedEgoStates++;
if (m_egoStateNode != NULL) {
Lock l(m_rootMutex);
EgoState egostate = c.getData<EgoState>();
Point3 dir(0, 0, egostate.getRotation().getAngleXY());
m_egoStateNode->setRotation(dir);
m_egoStateNode->setTranslation(egostate.getPosition());
Position egoPosition;
egoPosition.setPosition(egostate.getPosition());
egoPosition.setRotation(egostate.getRotation());
m_mapOfCurrentPositions[m_egoStateNodeDescriptor] = egoPosition;
if ( (m_numberOfReceivedEgoStates % 30) == 0 ) {
NodeDescriptor nd("EgoCar (Trace)");
TransformGroup *tg = m_mapOfTraceablePositions[nd];
if (tg != NULL) {
Point3 color(0, 0, 1);
hesperia::threeD::models::Point *p = new hesperia::threeD::models::Point(NodeDescriptor("Trace"), egostate.getPosition(), color, 5);
tg->addChild(p);
}
}
}
}
if (c.getDataType() == Container::CONTOUREDOBJECTS) {
if (m_contouredObjectsNode != NULL) {
Lock l(m_rootMutex);
ContouredObjects cos = c.getData<ContouredObjects>();
vector<ContouredObject> listOfContouredObjects = cos.getContouredObjects();
vector<ContouredObject>::iterator it = listOfContouredObjects.begin();
m_contouredObjectsNode->deleteAllChildren();
while (it != listOfContouredObjects.end()) {
vector<Point3> contour = (*it).getContour();
vector<Point3>::iterator jt = contour.begin();
while (jt != contour.end()) {
m_contouredObjectsNode->addChild(new hesperia::threeD::models::Point(NodeDescriptor("Point"), (*jt), Point3(1, 0, 0), 2));
jt++;
}
it++;
}
}
}
if (c.getDataType() == Container::ROUTE) {
if (m_plannedRoute != NULL) {
Lock l(m_rootMutex);
Route r = c.getData<Route>();
vector<Point3> listOfVertices = r.getListOfPoints();
const uint32_t SIZE = listOfVertices.size();
if (SIZE > 0) {
m_plannedRoute->deleteAllChildren();
for (uint32_t i = 0; i < SIZE - 1; i++) {
Point3 posA = listOfVertices.at(i);
posA.setZ(0.05);
Point3 posB = listOfVertices.at(i+1);
posB.setZ(0.05);
m_plannedRoute->addChild(new hesperia::threeD::models::Line(NodeDescriptor(), posA, posB, Point3(0, 1, 0), 6));
}
}
}
}
if (c.getDataType() == Container::DRAW_LINE) {
if (m_lines != NULL) {
Lock l(m_rootMutex);
hesperia::data::environment::Line line = c.getData<Line>();
Point3 posA = line.getA();
posA.setZ(0.05);
Point3 posB = line.getB();
posB.setZ(0.05);
m_lines->addChild(new hesperia::threeD::models::Line(NodeDescriptor(), posA, posB, Point3(1, 0, 0), 6));
}
}
if (c.getDataType() == Container::OBSTACLE) {
if (m_obstaclesRoot != NULL) {
Lock l(m_rootMutex);
Obstacle obstacle = c.getData<Obstacle>();
switch (obstacle.getState()) {
case Obstacle::REMOVE:
{
// Remove obstacle.
map<uint32_t, Node*>::iterator result = m_mapOfObstacles.find(obstacle.getID());
if (result != m_mapOfObstacles.end()) {
// Remove child from scene graph node.
m_obstaclesRoot->removeChild(result->second);
// Remove entry from map.
m_mapOfObstacles.erase(result);
}
}
break;
case Obstacle::UPDATE:
{
map<uint32_t, Node*>::iterator result = m_mapOfObstacles.find(obstacle.getID());
if (result != m_mapOfObstacles.end()) {
// Remove child from scene graph node.
m_obstaclesRoot->removeChild(result->second);
// Remove entry from map.
m_mapOfObstacles.erase(result);
}
// Update obstacle.
TransformGroup *contourTG = new TransformGroup();
vector<Point3> contour = obstacle.getPolygon().getVertices();
// Close polygons.
Point3 p = contour.at(0);
contour.push_back(p);
for (uint32_t k = 0; k < contour.size() - 1; k++) {
Point3 A = contour.at(k); A.setZ(0.5);
Point3 B = contour.at(k+1); B.setZ(0.5);
contourTG->addChild(new hesperia::threeD::models::Line(NodeDescriptor(), A, B, Point3(0, 1, 0), 2));
}
m_mapOfObstacles[obstacle.getID()] = contourTG;
m_obstaclesRoot->addChild(contourTG);
}
break;
}
}
}
}
