// Distribute VehicleData. Takes a string as input.
        void Proxy::vdDistribute(const string decodedData){
          VehicleData vd;

          double cmTrav;
          bool converted = false;

          size_t index1 = decodedData.find("WP");

          try {
            cmTrav = stoi(decodedData.substr(index1 + 2));
            converted = true;
          }
          catch (std::invalid_argument&){
            cerr << "STOI: Invalid Arguments." << endl;
          }
          catch (std::out_of_range&){
            cerr << "STOI: Out of range." << endl;
          }
          if (converted){
            vd.setAbsTraveledPath(cmTrav);
            converted = false;
          }

          cout << "cmTrav: " << vd.getAbsTraveledPath() << endl;

          Container c3(vd);
          getConference().send(c3);
        }
Exemple #2
0
odcore::data::dmcp::ModuleExitCodeMessage::ModuleExitCode Overtake::body() {

    while (getModuleStateAndWaitForRemainingTimeInTimeslice() == odcore::data::dmcp::ModuleStateMessage::RUNNING) {
        // 1. Get most recent vehicle data:
        Container containerVehicleData = getKeyValueDataStore().get(automotive::VehicleData::ID());
        VehicleData vd = containerVehicleData.getData<VehicleData> ();

        // 2. Get most recent sensor board data describing virtual sensor data:
        Container containerSensorBoardData = getKeyValueDataStore().get(automotive::miniature::SensorBoardData::ID());
        SensorBoardData sbd = containerSensorBoardData.getData<SensorBoardData> ();

        // Create vehicle control data.
        VehicleControl vc;

        cout << "Overtake: "  << vd.getSpeed() << endl;

        cout << "This is the amount of sensors: " << sbd.getNumberOfSensors() << endl;

        // Create container for finally sending the data.
        //Container c(Container::VEHICLECONTROL, vc);
        // Send container.
        //getConference().send(c);
        cout << "This is printed" << endl;

    }
    return odcore::data::dmcp::ModuleExitCodeMessage::OKAY;
}
    ModuleState::MODULE_EXITCODE Vehicle::runLinearBicycleModel() {
        LinearBicycleModel lbm(getKeyValueConfiguration());

        KeyValueDataStore &kvs = getKeyValueDataStore();

        while (getModuleState() == ModuleState::RUNNING) {
            // Get current ForceControl.
            Container c = kvs.get(Container::FORCECONTROL);
            ForceControl fc = c.getData<ForceControl>();
            cerr << "ForceControl: '" << fc.toString() << "'" << endl;

        	lbm.accelerate(fc.getAccelerationForce());
        	lbm.brake(fc.getBrakeForce());
        	lbm.steer(fc.getSteeringForce());

            if (fc.getBrakeLights()) {
                cout << "Turn ON brake lights." << endl;
            }
            else {
            }

            if (fc.getLeftFlashingLights()) {
                cout << "Turn ON left flashing lights." << endl;
            }
            else {
            }

            if (fc.getRightFlashingLights()) {
                cout << "Turn ON right flashing lights." << endl;
            }
            else {
            }
           
            EgoState es = lbm.computeEgoState();
            
            // Get vehicle data.
            VehicleData vd = lbm.getVehicleData();
            cerr << "VehicleData: '" << vd.toString() << "'" << endl;

            Container container(Container::EGOSTATE, es);
            getConference().send(container);

            // Send vehicledata.
            Container c2(Container::VEHICLEDATA, vd);
            getConference().send(c2);
        }
        return ModuleState::OKAY;
    }
    ModuleState::MODULE_EXITCODE Vehicle::runLinearBicycleModelNew(const bool &withSpeedController) {
        LinearBicycleModelNew lbmn(getKeyValueConfiguration(), withSpeedController);

        KeyValueDataStore &kvs = getKeyValueDataStore();

        while (getModuleState() == ModuleState::RUNNING) {
            // Get current ForceControl.
            Container c = kvs.get(Container::VEHICLECONTROL);
            VehicleControl vc = c.getData<VehicleControl>();
            cerr << "VehicleControl: '" << vc.toString() << "'" << endl;

            if (withSpeedController) {
            	lbmn.speed(vc.getSpeed());
            }
            else {
            	lbmn.accelerate(vc.getAcceleration());
            }
        	lbmn.steer(vc.getSteeringWheelAngle());

            if (vc.getBrakeLights()) {
                cout << "Turn ON brake lights." << endl;
            }

            if (vc.getLeftFlashingLights()) {
                cout << "Turn ON left flashing lights." << endl;
            }

            if (vc.getRightFlashingLights()) {
                cout << "Turn ON right flashing lights." << endl;
            }
           
            EgoState es = lbmn.computeEgoState();
            
            // Get vehicle data.
            VehicleData vd = lbmn.getVehicleData();
            cerr << "VehicleData: '" << vd.toString() << "'" << endl;

            Container container(Container::EGOSTATE, es);
            getConference().send(container);

            // Send vehicledata.
            Container c2(Container::VEHICLEDATA, vd);
            getConference().send(c2);
        }
        return ModuleState::OKAY;
    }
    /* ----------------------------------- Data ---------------------------- */
    void AutodriveSim::updateAutodriveData(){
    
        // Vehicle data
        Container containerVehicleData = getKeyValueDataStore().get(Container::VEHICLEDATA);
        VehicleData vd = containerVehicleData.getData<VehicleData> ();

        // Sensor board data
        Container containerSensorBoardData = getKeyValueDataStore().get(Container::USER_DATA_0);
        SensorBoardData sbd = containerSensorBoardData.getData<SensorBoardData> ();

        // Assigning the values of the sensors
        Autodrive::SensorData::infrared.frontright = sbd.getMapOfDistances()[0];
        Autodrive::SensorData::infrared.rear = sbd.getMapOfDistances()[1];
        Autodrive::SensorData::infrared.rearright = sbd.getMapOfDistances()[2];
        Autodrive::SensorData::ultrasound.front = sbd.getMapOfDistances()[3];
        Autodrive::SensorData::ultrasound.frontright = sbd.getMapOfDistances()[4];
        Autodrive::SensorData::ultrasound.rear = sbd.getMapOfDistances()[5];

        // Assigning the values of the vehicle
        Autodrive::SensorData::encoderPulses = vd.getAbsTraveledPath();
        Autodrive::SensorData::gyroHeading = vd.getHeading() * Constants::RAD2DEG;
    }
Exemple #6
0
        // This method will do the main data processing job.
        coredata::dmcp::ModuleExitCodeMessage::ModuleExitCode BoxParker::body() {
            double distanceOld = 0;
            double absPathStart = 0;
            double absPathEnd = 0;

            int stageMoving = 0;
            int stageMeasuring = 0;

            while (getModuleStateAndWaitForRemainingTimeInTimeslice() == coredata::dmcp::ModuleStateMessage::RUNNING) {
                // 1. Get most recent vehicle data:
                Container containerVehicleData = getKeyValueDataStore().get(Container::VEHICLEDATA);
                VehicleData vd = containerVehicleData.getData<VehicleData> ();

                // 2. Get most recent sensor board data describing virtual sensor data:
                Container containerSensorBoardData = getKeyValueDataStore().get(Container::USER_DATA_0);
                SensorBoardData sbd = containerSensorBoardData.getData<SensorBoardData> ();

                // Create vehicle control data.
                VehicleControl vc;

                // Moving state machine.
                if (stageMoving == 0) {
                    // Go forward.
                    vc.setSpeed(1);
                    vc.setSteeringWheelAngle(0);
                }
                if ((stageMoving > 0) && (stageMoving < 20)) {
                    // Move slightly forward.
                    vc.setSpeed(1);
                    vc.setSteeringWheelAngle(0);
                    stageMoving++;
                }
                if ((stageMoving >= 20) && (stageMoving < 25)) {
                    // Stop.
                    vc.setSpeed(0);
                    vc.setSteeringWheelAngle(0);
                    stageMoving++;
                }
                if ((stageMoving >= 25) && (stageMoving < 80)) {
                    // Backwards, steering wheel to the right.
                    vc.setSpeed(-2);
                    vc.setSteeringWheelAngle(25);
                    stageMoving++;
                }
                if (stageMoving >= 80) {
                    // Stop.
                    vc.setSpeed(0);
                    vc.setSteeringWheelAngle(0);

                    stageMoving++;
                }
                if (stageMoving >= 150) {
                    // End component.
                    break;
                }

                // Measuring state machine.
                switch (stageMeasuring) {
                    case 0:
                        {
                            // Initialize measurement.
                            distanceOld = sbd.getValueForKey_MapOfDistances(2);
                            stageMeasuring++;
                        }
                    break;
                    case 1:
                        {
                            // Checking for distance sequence +, -.
                            if ((distanceOld > 0) && (sbd.getValueForKey_MapOfDistances(2) < 0)) {
                                // Found distance sequence +, -.
                                stageMeasuring = 2;
                                absPathStart = vd.getAbsTraveledPath();
                            }
                            distanceOld = sbd.getValueForKey_MapOfDistances(2);
                        }
                    break;
                    case 2:
                        {
                            // Checking for distance sequence -, +.
                            if ((distanceOld < 0) && (sbd.getValueForKey_MapOfDistances(2) > 0)) {
                                // Found distance sequence -, +.
                                stageMeasuring = 1;
                                absPathEnd = vd.getAbsTraveledPath();

                                const double GAP_SIZE = (absPathEnd - absPathStart);
                                cerr << "Size = " << GAP_SIZE << endl;
                                m_foundGaps.push_back(GAP_SIZE);

                                if ((stageMoving < 1) && (GAP_SIZE > 3.5)) {
                                    stageMoving = 1;
                                }
                            }
                            distanceOld = sbd.getValueForKey_MapOfDistances(2);
                        }
                    break;
                }

                // Create container for finally sending the data.
                Container c(Container::VEHICLECONTROL, vc);
                // Send container.
                getConference().send(c);
            }

            return coredata::dmcp::ModuleExitCodeMessage::OKAY;
        }