Пример #1
0
 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;
     }
 }
Пример #2
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;
    }
Пример #3
0
 void Data2StringStream::toStringStream(const EgoState &es) {
     m_sstr << es.toString();
 }
        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);
                }
            }
        }