コード例 #1
0
        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();
                }
            }
        }
コード例 #2
0
ファイル: IRUS.cpp プロジェクト: DKarlberg/OpenDaVINCI
        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;
        }
コード例 #3
0
    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);
        }
    }
コード例 #4
0
ファイル: IRUS.cpp プロジェクト: tomkek/projects
    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;
    }
コード例 #5
0
            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;
                        }
                    }
                }
            }