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 StreetMapMapWidget::nextContainer(Container &c) {
     if (c.getDataType() == opendlv::data::environment::EgoState::ID()) {
         EgoState es = c.getData<EgoState>();
         cout << "[StreetMapMapWidget]: " << es.toString() << endl;
     }
     else {
         cout << "[StreetMapMapWidget]: Received container " << c.getDataType() << endl;
     }
 }
Beispiel #3
0
        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);
        }
    }
Beispiel #5
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();
                }
            }
        }
Beispiel #6
0
    coredata::dmcp::ModuleExitCodeMessage::ModuleExitCode DrivenPath::body() {
        KeyValueDataStore &kvs = getKeyValueDataStore();

        // Get scenario.
        const URL urlOfSCNXFile(getKeyValueConfiguration().getValue<string>("global.scenario"));
        
        // Create graph.
        core::wrapper::graph::DirectedGraph m_graph;

        // Read scenario.
        vector<NamedLine> listOfLines;
        if (urlOfSCNXFile.isValid()) {
            SCNXArchive &scnxArchive = SCNXArchiveFactory::getInstance().getSCNXArchive(urlOfSCNXFile);

            hesperia::data::scenario::Scenario &scenario = scnxArchive.getScenario();

            // Construct road network.
            LaneVisitor lv(m_graph, scenario);
            scenario.accept(lv);

            listOfLines = lv.getListOfLines();
        }

        
        vector<NamedLine>::const_iterator it = listOfLines.begin();
        while (it != listOfLines.end()) {
            NamedLine l =(*it++);

            // Draw line.
            Container c = Container(Container::DRAW_LINE, l);
            getConference().send(c);
/*
            cout << "Lines;" << l.getName() << ";" << l.getA().getX()
                                            << ";" << l.getA().getY()
                                            << ";" << l.getA().getZ()
                                            << ";" << l.getB().getX()
                                            << ";" << l.getB().getY()
                                            << ";" << l.getB().getZ() << endl;
*/
            cout << "LinesA;" << l.getName() << ";" << l.getA().getX()
                                            << ";" << l.getA().getY()
                                            << ";" << l.getA().getZ() << endl;
            cout << "LinesB;" << l.getName() << ";" << l.getB().getX()
                                            << ";" << l.getB().getY()
                                            << ";" << l.getB().getZ() << endl;
        }


        unsigned int counter = 0;

        while (getModuleStateAndWaitForRemainingTimeInTimeslice() == coredata::dmcp::ModuleStateMessage::RUNNING) {
            // Get current ego state.
            Container c = kvs.get(Container::EGOSTATE);
            EgoState es = c.getData<EgoState>();
            cout << counter << ";EgoState: '" << es.toString() << "'" << endl;
            cout << counter << ";EgoState-short;" << es.getPosition().getX() << ";" << es.getPosition().getY() << ";" << es.getPosition().getZ() << endl;

            // Algorithm:
            // 1. Get position of vehicle.
            // 2. Calculate perpendicular point to all lines in the list.
            // 3. Determine the distance between the position and the perpendicular point.
            // 4. Iterate through all named lines and determine the smallest distance.
            // 5. Print the smallest distance/named line.

            string oldName = "";
            double oldDistance = numeric_limits<double>::max();
            string oldNameOverall = "";
            double oldDistanceOverall = numeric_limits<double>::max();
            it = listOfLines.begin();
            while (it != listOfLines.end()) {
                NamedLine l =(*it++);
                if (l.getName().compare(oldName) != 0) {
                    // Print results:
                    if (oldName.compare("") != 0) {
                        cout << counter << ";Distance;" << oldName << ";" << oldDistance << endl;
                    }

                    // Reset values because we are starting a new turn.
                    oldDistance = numeric_limits<double>::max();
                }

                Point3 vehiclePosition = es.getPosition();
                Point3 perpendicularPoint = l.getPerpendicularPoint(vehiclePosition);

                // Compute distance between vehicle's position and lane segment.
                double length = (perpendicularPoint - vehiclePosition).lengthXY();
                if (length < oldDistance) {
                    oldDistance = length;
                }

                if (length < oldDistanceOverall) {
                    oldDistanceOverall = length;
                    oldNameOverall = l.getName();
                }

                oldName = l.getName();
            }

            cout << counter << ";DistanceOverall;" << oldNameOverall << ";" << oldDistanceOverall << endl;

            counter++;
        }
        return coredata::dmcp::ModuleExitCodeMessage::OKAY;
    }
Beispiel #7
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 Data2StringStream::toStringStream(const EgoState &es) {
     m_sstr << es.toString();
 }
            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;
                        }
                    }
                }
            }