void DataRenderer::draw(const ContouredObjects &cos) {
Lock l(m_dataRendererMutex);
if (m_renderer != NULL) {
vector<ContouredObject> listOfContouredObjects = cos.getContouredObjects();
vector<ContouredObject>::iterator it = listOfContouredObjects.begin();
while (it != listOfContouredObjects.end()) {
m_renderer->beginPainting();
switch (cos.getColor()) {
case ContouredObjects::BLUE:
m_renderer->setColor(Point3(0, 0, 1));
break;
case ContouredObjects::GREEN:
m_renderer->setColor(Point3(0, 1, 0));
break;
case ContouredObjects::RED:
m_renderer->setColor(Point3(1, 0, 0));
break;
case ContouredObjects::YELLOW:
m_renderer->setColor(Point3(1, 1, 0));
break;
}
m_renderer->setPointWidth(1.0);
vector<Point3> contour = (*it).getContour();
vector<Point3>::iterator jt = contour.begin();
while (jt != contour.end()) {
m_renderer->drawPoint(*jt++);
}
m_renderer->endPainting();
it++;
}
}
}
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;
}
}
}
}