// 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);
        }
    /* ----------------------------------- 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 #3
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;
        }