C++ Obstacle::getPolygon方法代码示例

        void DistanceToObjectsReport::report(const odcore::wrapper::Time &t) {
            cerr << "Call to DistanceToObjectsReport for t = " << t.getSeconds() << "." << t.getPartialMicroseconds() << ", containing " << getFIFO().getSize() << " containers." << endl;

            // Get last EgoState.
            KeyValueDataStore &kvds = getKeyValueDataStore();
            Container c = kvds.get(opendlv::data::environment::EgoState::ID());
            EgoState es = c.getData<EgoState>();

            const uint32_t SIZE = getFIFO().getSize();
            for (uint32_t i = 0; i < SIZE; i++) {
                c = getFIFO().leave();
                cerr << "Received: " << c.toString() << endl;

                if (c.getDataType() == opendlv::data::environment::Obstacle::ID()) {
                    Obstacle o = c.getData<Obstacle>();

                    const float DISTANCE = (es.getPosition().getDistanceTo(o.getPosition()));
                    cerr << "DistanceToObjectsReport: Distance to object: " << DISTANCE << ", E: " << es.toString() << ", o: " << o.getPosition().toString() << endl;

                    // Continuously check distance.
                    m_correctDistance &= (DISTANCE > m_threshold);

                    vector<Point3> shape = o.getPolygon().getVertices();
                    Point3 head = shape.front();
                    shape.push_back(head);
                    const uint32_t NUMVERTICES = shape.size();
                    for(uint32_t j = 1; j < NUMVERTICES; j++) {
                        Point3 pA = shape.at(j-1);
                        Point3 pB = shape.at(j);

                        // TODO: Check polygonal data as well as perpendicular to all sides.
                        // Create line.
                        Line l(pA, pB);

                        // Compute perpendicular point.
                        Point3 perpendicularPoint = l.getPerpendicularPoint(es.getPosition());

                        // Compute distance between current position and perpendicular point.
                        const float DISTANCE_PP = (es.getPosition().getDistanceTo(perpendicularPoint));

                        cerr << "DistanceToObjectsReport: Distance to object's shape: " << DISTANCE_PP << ", E: " << es.toString() << ", o: " << o.getPosition().toString() << ", perpendicular point:" << perpendicularPoint.toString() << endl;

                        // Continuously check distance.
                        m_correctDistance &= (DISTANCE > m_threshold);
                    }
                }

                if (c.getDataType() == opendlv::data::environment::OtherVehicleState::ID()) {
                    OtherVehicleState o = c.getData<OtherVehicleState>();

                    const float DISTANCE = (es.getPosition().getDistanceTo(o.getPosition()));

                    // Compute distance between current position and perpendicular point.
                    cerr << "DistanceToObjectsReport: Distance to other vehicle: " << DISTANCE << ", E: " << es.toString() << ", o: " << o.getPosition().toString() << endl;

                    // Continuously check distance.
                    m_correctDistance &= (DISTANCE > m_threshold);
                }
            }
        }

        void DataRenderer::draw(const Obstacle &o) {
            Lock l(m_dataRendererMutex);
            if (m_renderer != NULL) {
                // Ignore other states.
                if (o.getState() == Obstacle::UPDATE) {
                    // Update obstacle.
                    m_renderer->beginPainting();
                        m_renderer->setColor(Point3(0, 1, 0));
                        m_renderer->setLineWidth(2.0);

                        // Add additional height to lift the obstacle from the ground.
                        vector<Point3> points;
                        vector<Point3> originalPoints = o.getPolygon().getVertices();
                        vector<Point3>::iterator it = originalPoints.begin();
                        while (it != originalPoints.end()) {
                            Point3 p = (*it++);
                            p.setZ(0.5);
                            points.push_back(p);
                        }
                        m_renderer->drawPolyLine(points);
                    m_renderer->endPainting();
                }
            }
        }

    core::SharedPointer<core::wrapper::Image> OpenGLGrabber::getNextImage() {
        if ( (m_sharedMemory.isValid()) && (m_sharedMemory->isValid()) ) {
            m_sharedMemory->lock();

            // Render the image right before grabbing it.
            switch (m_render) {
                case  OpenGLGrabber::IN_CAR:
                    renderNextImageInCar();

                    m_FIFO_Obstacles.clear();
                break;
                case  OpenGLGrabber::CHASE_CAR:
                    renderNextImageChaseCar();

                    m_FIFO_Obstacles.clear();
                break;
                case  OpenGLGrabber::CHASE_CAR_SENSORS:
                {
                    const uint32_t size = m_FIFO_Obstacles.getSize();
                    for(uint32_t i = 0; i < size; i++) {
                        Container c = m_FIFO_Obstacles.leave();
                        if (c.getDataType() == Container::OBSTACLE) {
                            Obstacle obstacle = c.getData<Obstacle>();

                            // Check if sensor FOV-"Obstacle":
                            if (obstacle.getID() >= 9000) {
                                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_sensors->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_sensors->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_sensors->addChild(contourTG);
                                    }
                                    break;
                                }
                            }
                        }
                    }

                    renderNextImageChaseCarSensors();
                }
                break;
            }

            // TODO Read pixels using BGRA!!!
            glReadBuffer(GL_BACK);
            glPixelStorei(GL_PACK_ALIGNMENT, 1);
            glReadPixels(0, 0, m_image->getWidth(), m_image->getHeight(), GL_BGR, GL_UNSIGNED_BYTE, m_sharedMemory->getSharedMemory());

            // Flip the image horizontally.
            m_image->flipHorizontally();

            m_sharedMemory->unlock();
        }

        return m_image;
    }

//.........这里部分代码省略.........
                    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;
                        }
                    }
                }
            }