Ejemplo n.º 1
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;
}
        odcore::data::dmcp::ModuleExitCodeMessage::ModuleExitCode ViewLiveData::body() {
         // Parameters for overtaking.
            // const int32_t ULTRASONIC_FRONT_CENTER = 3;
            // const int32_t ULTRASONIC_FRONT_RIGHT = 4;
            // const int32_t INFRARED_FRONT_RIGHT = 0;
            const int32_t INFRARED_REAR_RIGHT = 2;

            while (getModuleStateAndWaitForRemainingTimeInTimeslice() == odcore::data::dmcp::ModuleStateMessage::RUNNING) {
                
                Container containerSensorBoardData = getKeyValueDataStore().get(automotive::miniature::SensorBoardData::ID());
                SensorBoardData sbd = containerSensorBoardData.getData<SensorBoardData> ();
                cout << "Most value of infrared rear right: '" << sbd.getValueForKey_MapOfDistances(INFRARED_REAR_RIGHT) << "'" << endl;

                double distance = sbd.getValueForKey_MapOfDistances(INFRARED_REAR_RIGHT);
                if(distance > 3){
                    distance = 0;
                }
                sbd.putTo_MapOfDistances(INFRARED_REAR_RIGHT, distance + 0.01 );
                
                cout << "NEXT value of infrared rear right: '" << sbd.getValueForKey_MapOfDistances(INFRARED_REAR_RIGHT) << "'" << endl;                

                Container c(sbd);
                getConference().send(c);
                cout << "done." << endl;
            }

            return odcore::data::dmcp::ModuleExitCodeMessage::OKAY;
        }
Ejemplo n.º 3
0
    /* ----------------------------------- 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;
    }
Ejemplo n.º 4
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;
        }
Ejemplo n.º 5
0
    // This method will do the main data processing job.
    ModuleState::MODULE_EXITCODE Proxy::body() {
     
        uint32_t captureCounter = 0;
        SensorBoardData sensorBoardData;
        core::data::environment::VehicleData vd;
        while (getModuleState() == ModuleState::RUNNING) {

            // Capture frame.
            if (m_camera != NULL) {
                core::data::image::SharedImage si = m_camera->capture();

                Container c(Container::SHARED_IMAGE, si);
                distribute(c);
                captureCounter++;
            }
	           Container containerVehicleControl = getKeyValueDataStore().get(Container::VEHICLECONTROL);
             VehicleControl vc = containerVehicleControl.getData<VehicleControl> ();
             
             cerr << "Speed data: " << vc.getSpeed() << endl;
             cout << "Angle : " << vc.getSteeringWheelAngle()<<endl;
            // TODO: Here, you need to implement the data links to the embedded system
            // to read data from IR/US.

             int angle=60;
             int angleFromDriver= (int)vc.getSteeringWheelAngle(); // receive steeringaAngle from VehicleControl

             //convert the angle for arduino
             if(angleFromDriver <0)
              angleFromDriver*=-1;
            else
               angleFromDriver*=-1;
             angle+=angleFromDriver;
             stringstream ss;
              ss << angle;
              if(angle<100 && angle >10)
             convertedAngle="0"+ss.str();
           else if(angle <10 && angle>-1)
            convertedAngle="00"+ss.str();
           else
            convertedAngle=ss.str();
          

          // send different values depending on drivers speed
            if(vc.getSpeed()>0)
              userInput="600"+convertedAngle+",";
            else if(vc.getSpeed()<1 && vc.getSpeed()>-1)
              userInput="512"+convertedAngle+",";
            else if(vc.getSpeed()<-1)
              userInput="200"+convertedAngle+",";
              
              
              cout<<userInput<<endl;

              if(wd!=-1 && wd!=0)
             msv::write(userInput); // write to arduino


      if(fd!=-1 && fd!=0){

  readings=msv::read(); // read from arduino
	
  cout<< "readings are "<< readings << endl;
  
  int length=atoi(readings.substr(0,2).c_str());
  unsigned int finalLength=length+5; // check length
  

  //decode netstring received from arduino
  if(readings.length()==finalLength){
    string ir1=readings.substr(3,3);

    valIr1=atoi(ir1.c_str());
  	
    string ir2=readings.substr(6,3);

    valIr2=atoi(ir2.c_str());
    
    string ir3=readings.substr(9,3);

    valIr3=atoi(ir3.c_str());
    
    string us1=readings.substr(12,3);

    valUs1=atoi(us1.c_str());
    
    string us2=readings.substr(15,3);

    valUs2=atoi(us2.c_str());
    
    string us3=readings.substr(18,3);

    valUs3=atoi(us3.c_str());

    string wheelE=readings.substr(21,length-18);

    valWheelE=atoi(wheelE.c_str());
    
}

  cout<<"Wheel Encoder value " << valWheelE <<endl;

//Map decoded sensor values
  sensorBoardData.putTo_MapOfDistances(4,valUs2);
  sensorBoardData.putTo_MapOfDistances(3,valUs3);
  sensorBoardData.putTo_MapOfDistances(1,valIr3);
  sensorBoardData.putTo_MapOfDistances(2,valIr2);
  sensorBoardData.putTo_MapOfDistances(0,valIr1);
  sensorBoardData.putTo_MapOfDistances(5,valUs1);
  vd.setAbsTraveledPath(valWheelE);

  Container Pvd=Container(Container::VEHICLEDATA, vd);
  Container c = Container(Container::USER_DATA_0, sensorBoardData);
  
  distribute(c);
  distribute(Pvd);

  tcflush(fd, TCIFLUSH);
}

if(wd!=-1 && wd!=0)
  tcflush(wd, TCOFLUSH);
//usleep(2000000);

 }  
        if(wd!=-1 && wd!=0){
        msv::write("512060,");
        msv::close(wd);
        }
        if(fd!=-1 && fd!=0)
        msv::close(fd);
        
        cout << "Proxy: Captured " << captureCounter << " frames." << endl;

        return ModuleState::OKAY;
    }
Ejemplo n.º 6
0
        // Distribute SensorBoardData. Takes a string as input.
        void Proxy::sbdDistribute(const string decodedData){
          SensorBoardData sbd;

          int irFrontRight;
          int irRearRight;
          int irRearCenter;
          int usFrontCenter;
          int usFrontRight;
          bool converted = false;

          size_t index1 = decodedData.find("IRFR");
          size_t index2 = decodedData.find("IRRR");
          size_t index3 = decodedData.find("IRRC");
          size_t index4 = decodedData.find("USF");
          size_t index5 = decodedData.find("USR");

          try {
            irFrontRight = stoi(decodedData.substr(index1 + 4));
            irRearRight = stoi(decodedData.substr(index2 + 4));
            irRearCenter = stoi(decodedData.substr(index3 + 4));
            usFrontCenter = stoi(decodedData.substr(index4 + 3));
            usFrontRight = stoi(decodedData.substr(index5 + 3));
            converted = true;
          }
          catch (std::invalid_argument&){
            cerr << "STOI: Invalid Arguments." << endl;
          }
          catch (std::out_of_range&){
            cerr << "STOI: Out of range." << endl;
          }
          if (converted){
            sbd.putTo_MapOfDistances(0, irFrontRight);
            sbd.putTo_MapOfDistances(1, irRearRight);
            sbd.putTo_MapOfDistances(2, irRearCenter);
            sbd.putTo_MapOfDistances(3, usFrontCenter);
            sbd.putTo_MapOfDistances(4, usFrontRight);
            converted = false;
          }


          cout << "irFrontRight: " << sbd.getValueForKey_MapOfDistances(0) << endl;
          cout << "irRearRight: " << sbd.getValueForKey_MapOfDistances(1) << endl;
          cout << "irRearCenter: " << sbd.getValueForKey_MapOfDistances(2) << endl;
          cout << "usFrontCenter: " << sbd.getValueForKey_MapOfDistances(3) << endl;
          cout << "usFrontRight: " << sbd.getValueForKey_MapOfDistances(4) << endl;

          Container c2(sbd);
          getConference().send(c2);
        }
        void LaneFollower::measuringMachine() {
            const int32_t ULTRASONIC_FRONT_CENTER = 3;
            const int32_t ULTRASONIC_FRONT_RIGHT = 4;
            const int32_t INFRARED_FRONT_RIGHT = 0;
            const int32_t INFRARED_REAR_RIGHT = 2;

            const double OVERTAKING_DISTANCE = 6; //use 6
            const double HEADING_PARALLEL = 0.01;
            
            // 1. Get most recent vehicle data:
            Container containerVehicleData = getKeyValueDataStore().get(VehicleData::ID());
            VehicleData vd = containerVehicleData.getData<VehicleData> ();

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

            if (stageMeasuring == FIND_OBJECT_INIT) {
                distanceToObstacleOld = sbd.getValueForKey_MapOfDistances(ULTRASONIC_FRONT_CENTER);
                stageMeasuring = FIND_OBJECT;
            }
            
            else if (stageMeasuring == FIND_OBJECT) {
                distanceToObstacle = sbd.getValueForKey_MapOfDistances(ULTRASONIC_FRONT_CENTER);

                // Approaching an obstacle (stationary or driving slower than us).
                if ( (distanceToObstacle > 0) && (((distanceToObstacleOld - distanceToObstacle) > 0) || (fabs(distanceToObstacleOld - distanceToObstacle) < 1e-2)) ) {
                    // Check if overtaking shall be started.
                    stageMeasuring = FIND_OBJECT_PLAUSIBLE;
                }

                distanceToObstacleOld = distanceToObstacle;
            }
            
            else if (stageMeasuring == FIND_OBJECT_PLAUSIBLE) {
                if (sbd.getValueForKey_MapOfDistances(ULTRASONIC_FRONT_CENTER) < OVERTAKING_DISTANCE && distanceToObstacleOld < OVERTAKING_DISTANCE ) {
                    stageMoving = TO_LEFT_LANE_LEFT_TURN;
                    overtake = true;
                    cerr << "===========================OVERTAKE=================="  << endl;

                    // Disable measuring until requested from moving state machine again.
                    stageMeasuring = DISABLE;
                }
                else {
                    stageMeasuring = FIND_OBJECT;
                }
            }
            
            else if (stageMeasuring == HAVE_BOTH_IR) {
                // Remain in this stage until both IRs see something.
                if ( (sbd.getValueForKey_MapOfDistances(INFRARED_FRONT_RIGHT) > 0) && (sbd.getValueForKey_MapOfDistances(INFRARED_REAR_RIGHT) > 0) ) {
                    // Turn to right.
                    stageMoving = TO_LEFT_LANE_RIGHT_TURN;
                }
            }
    
            else if (stageMeasuring == HAVE_BOTH_IR_SAME_DISTANCE) {
                // Remain in this stage until both IRs have the similar distance to obstacle (i.e. turn car)
                // and the driven parts of the turn are plausible.
                const double IR_FR = sbd.getValueForKey_MapOfDistances(INFRARED_FRONT_RIGHT);
                const double IR_RR = sbd.getValueForKey_MapOfDistances(INFRARED_REAR_RIGHT);

                if ((fabs(IR_FR - IR_RR) < HEADING_PARALLEL) && ((stageToRightLaneLeftTurn - stageToRightLaneRightTurn) > 20)) {
                     // Straight forward again.
                    stageMoving = CONTINUE_ON_LEFT_LANE;
                }
            }
        
            else if (stageMeasuring == END_OF_OBJECT) {
                // Find end of object.
                distanceToObstacleOld = distanceToObstacle;
                distanceToObstacle = sbd.getValueForKey_MapOfDistances(ULTRASONIC_FRONT_RIGHT);

                if (distanceToObstacle < 0 && distanceToObstacleOld < 0) {
                    // Move to right lane again.
                    stageMoving = TO_RIGHT_LANE_RIGHT_TURN;

                    // Disable measuring until requested from moving state machine again.
                    stageMeasuring = DISABLE;
                }
            }
        }
Ejemplo n.º 8
0
        // This method will do the main data processing job.
        coredata::dmcp::ModuleExitCodeMessage::ModuleExitCode Overtaker::body() {
            const int32_t ULTRASONIC_FRONT_CENTER = 3;
            const int32_t ULTRASONIC_FRONT_RIGHT = 4;
            const int32_t INFRARED_FRONT_RIGHT = 0;
            const int32_t INFRARED_REAR_RIGHT = 2;

            const double OVERTAKING_DISTANCE = 5.5;
            const double HEADING_PARALLEL = 0.04;

            // Overall state machines for moving and measuring.
            enum StateMachineMoving { FORWARD, TO_LEFT_LANE_LEFT_TURN, TO_LEFT_LANE_RIGHT_TURN, CONTINUE_ON_LEFT_LANE, TO_RIGHT_LANE_RIGHT_TURN, TO_RIGHT_LANE_LEFT_TURN };
            enum StateMachineMeasuring { DISABLE, FIND_OBJECT_INIT, FIND_OBJECT, FIND_OBJECT_PLAUSIBLE, HAVE_BOTH_IR, HAVE_BOTH_IR_SAME_DISTANCE, END_OF_OBJECT };

            StateMachineMoving stageMoving = FORWARD;
            StateMachineMeasuring stageMeasuring = FIND_OBJECT_INIT;

            // State counter for dynamically moving back to right lane.
            int32_t stageToRightLaneRightTurn = 0;
            int32_t stageToRightLaneLeftTurn = 0;

            // Distance variables to ensure we are overtaking only stationary or slowly driving obstacles.
            double distanceToObstacle = 0;
            double distanceToObstacleOld = 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:
	            Container containerSensorBoardData = getKeyValueDataStore().get(Container::USER_DATA_0);
	            SensorBoardData sbd = containerSensorBoardData.getData<SensorBoardData> ();

	            // Create vehicle control data.
	            VehicleControl vc;

                // Moving state machine.
                if (stageMoving == FORWARD) {
                    // Go forward.
                    vc.setSpeed(2);
                    vc.setSteeringWheelAngle(0);

                    stageToRightLaneLeftTurn = 0;
                    stageToRightLaneRightTurn = 0;
                }
                else if (stageMoving == TO_LEFT_LANE_LEFT_TURN) {
                    // Move to the left lane: Turn left part until both IRs see something.
                    vc.setSpeed(1);
                    vc.setSteeringWheelAngle(-25);

                    // State machine measuring: Both IRs need to see something before leaving this moving state.
                    stageMeasuring = HAVE_BOTH_IR;

                    stageToRightLaneRightTurn++;
                }
                else if (stageMoving == TO_LEFT_LANE_RIGHT_TURN) {
                    // Move to the left lane: Turn right part until both IRs have the same distance to obstacle.
                    vc.setSpeed(1);
                    vc.setSteeringWheelAngle(25);

                    // State machine measuring: Both IRs need to have the same distance before leaving this moving state.
                    stageMeasuring = HAVE_BOTH_IR_SAME_DISTANCE;

                    stageToRightLaneLeftTurn++;
                }
                else if (stageMoving == CONTINUE_ON_LEFT_LANE) {
                    // Move to the left lane: Passing stage.
                    vc.setSpeed(2);
                    vc.setSteeringWheelAngle(0);

                    // Find end of object.
                    stageMeasuring = END_OF_OBJECT;
                }
                else if (stageMoving == TO_RIGHT_LANE_RIGHT_TURN) {
                    // Move to the right lane: Turn right part.
                    vc.setSpeed(1.5);
                    vc.setSteeringWheelAngle(25);

                    stageToRightLaneRightTurn--;
                    if (stageToRightLaneRightTurn == 0) {
                        stageMoving = TO_RIGHT_LANE_LEFT_TURN;
                    }
                }
                else if (stageMoving == TO_RIGHT_LANE_LEFT_TURN) {
                    // Move to the left lane: Turn left part.
                    vc.setSpeed(.9);
                    vc.setSteeringWheelAngle(-25);

                    stageToRightLaneLeftTurn--;
                    if (stageToRightLaneLeftTurn == 0) {
                        // Start over.
                        stageMoving = FORWARD;
                        stageMeasuring = FIND_OBJECT_INIT;

                        distanceToObstacle = 0;
                        distanceToObstacleOld = 0;
                    }
                }

                // Measuring state machine.
                if (stageMeasuring == FIND_OBJECT_INIT) {
                    distanceToObstacleOld = sbd.getValueForKey_MapOfDistances(ULTRASONIC_FRONT_CENTER);
                    stageMeasuring = FIND_OBJECT;
                }
                else if (stageMeasuring == FIND_OBJECT) {
                    distanceToObstacle = sbd.getValueForKey_MapOfDistances(ULTRASONIC_FRONT_CENTER);

                    // Approaching an obstacle (stationary or driving slower than us).
                    if ( (distanceToObstacle > 0) && (((distanceToObstacleOld - distanceToObstacle) > 0) || (fabs(distanceToObstacleOld - distanceToObstacle) < 1e-2)) ) {
                        // Check if overtaking shall be started.                        
                        stageMeasuring = FIND_OBJECT_PLAUSIBLE;
                    }

                    distanceToObstacleOld = distanceToObstacle;
                }
                else if (stageMeasuring == FIND_OBJECT_PLAUSIBLE) {
                    if (sbd.getValueForKey_MapOfDistances(ULTRASONIC_FRONT_CENTER) < OVERTAKING_DISTANCE) {
                        stageMoving = TO_LEFT_LANE_LEFT_TURN;

                        // Disable measuring until requested from moving state machine again.
                        stageMeasuring = DISABLE;
                    }
                    else {
                        stageMeasuring = FIND_OBJECT;
                    }
                }
                else if (stageMeasuring == HAVE_BOTH_IR) {
                    // Remain in this stage until both IRs see something.
                    if ( (sbd.getValueForKey_MapOfDistances(INFRARED_FRONT_RIGHT) > 0) && (sbd.getValueForKey_MapOfDistances(INFRARED_REAR_RIGHT) > 0) ) {
                        // Turn to right.
                        stageMoving = TO_LEFT_LANE_RIGHT_TURN;
                    }
                }
                else if (stageMeasuring == HAVE_BOTH_IR_SAME_DISTANCE) {
                    // Remain in this stage until both IRs have the similar distance to obstacle (i.e. turn car)
                    // and the driven parts of the turn are plausible.
                    const double IR_FR = sbd.getValueForKey_MapOfDistances(INFRARED_FRONT_RIGHT);
                    const double IR_RR = sbd.getValueForKey_MapOfDistances(INFRARED_REAR_RIGHT);

                    if ((fabs(IR_FR - IR_RR) < HEADING_PARALLEL) && ((stageToRightLaneLeftTurn - stageToRightLaneRightTurn) > 0)) {
                        // Straight forward again.
                        stageMoving = CONTINUE_ON_LEFT_LANE;
                    }
                }
                else if (stageMeasuring == END_OF_OBJECT) {
                    // Find end of object.
                    distanceToObstacle = sbd.getValueForKey_MapOfDistances(ULTRASONIC_FRONT_RIGHT);

                    if (distanceToObstacle < 0) {
                        // Move to right lane again.
                        stageMoving = TO_RIGHT_LANE_RIGHT_TURN;

                        // Disable measuring until requested from moving state machine again.
                        stageMeasuring = DISABLE;
                    }
                }

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

            return coredata::dmcp::ModuleExitCodeMessage::OKAY;
        }
Ejemplo n.º 9
0
        int Proxy::processCarString(const string &s){
            // Load configuration
            // TODO move it?
            KeyValueConfiguration kv = getKeyValueConfiguration();
            // TODO Use vectors and sensor_count ?(tokens[5] and arr_size)
            const uint32_t sensor_count = kv.getValue<uint32_t>("proxy.numberOfSensors");

            const uint LENGTH_RULE = kv.getValue<uint>("proxy.arduinoStringLength");

            const string sensor0_token = kv.getValue<string>("proxy.sensor0.token");
            const string sensor1_token = kv.getValue<string>("proxy.sensor1.token");
            const string sensor2_token = kv.getValue<string>("proxy.sensor2.token");
            const string sensor3_token = kv.getValue<string>("proxy.sensor3.token");
            const string sensor4_token = kv.getValue<string>("proxy.sensor4.token");

            const int sensor0_id = kv.getValue<int>("proxy.sensor0.id");;
            const int sensor1_id = kv.getValue<int>("proxy.sensor1.id");;
            const int sensor2_id = kv.getValue<int>("proxy.sensor2.id");;
            const int sensor3_id = kv.getValue<int>("proxy.sensor3.id");;
            const int sensor4_id = kv.getValue<int>("proxy.sensor4.id");;

            string payload = s;
            // Use tokens[sensor_count] instead of tokens[5]. Need to learn vector and its operators.
            string tokens[5] = {sensor0_token, sensor1_token, sensor2_token, sensor3_token, sensor4_token};

            int arr_size = sizeof(tokens)/sizeof(tokens[0]);
            size_t idx;
            int i;
            int digits;
            int problem;

            Container containerSensorBoardData = getKeyValueDataStore().get(automotive::miniature::SensorBoardData::ID());
            SensorBoardData sbd = containerSensorBoardData.getData<SensorBoardData> ();

            // Set the number of sensors to sensor_count
            sbd.setNumberOfSensors(sensor_count);

            // Check length
            if(payload.length() != LENGTH_RULE){
                //cout << "BAD LENGTH SHOULD FAIL" << endl;
            }
            
            // For each token
            for(i=0;i<arr_size;i++){
                problem = 0;

                string key = payload.substr(0,tokens[i].length());    

                //cout << i << ". Token=" << tokens[i] << " key=" << key << endl;

                if(key == tokens[i]){
                    payload = payload.substr(tokens[i].length()+1,string::npos); // Remove token and space

                    try {
                        digits = stoi(payload, &idx, 10); // get number
                    } catch (...) {
                        problem = 1;
                    }

                } else {
                    return -2;
                }
                // TODO fix ifs
                if(!problem){                    
                    // Add to SBD
                    //cout << "We parsed token:" << tokens[i] << " and got value:" << digits << endl;
                    
                    if(tokens[i] == sensor0_token){
                        sbd.putTo_MapOfDistances(sensor0_id, digits);
                    } else
                    if(tokens[i] == sensor1_token){
                        sbd.putTo_MapOfDistances(sensor1_id, digits);
                    } else
                    if(tokens[i] == sensor2_token){
                        sbd.putTo_MapOfDistances(sensor2_id, digits );
                    } else
                    if(tokens[i] == sensor3_token){
                        sbd.putTo_MapOfDistances(sensor3_id, digits );
                    } else
                    if(tokens[i] == sensor4_token){
                        sbd.putTo_MapOfDistances(sensor4_id, digits );
                    }

                    if(i < arr_size - 1){
                    payload = payload.substr(idx + 1,string::npos); // We need to remove digit values + a space
                    } else {
                        payload = payload.substr(idx,string::npos); // We only need to remove digit values
                    }

                } else {
                    return -3;
                }

            }

            if(payload.length() == 0){
                //cout << "Finished parsing: " << sbd.toString() << endl;
                // TODO Invoke something with SBD instead of sending it from here.
                Container sbdc(sbd);
                getConference().send(sbdc);
                return 1;
            } else {
                return 0; // TO DO: Should never happen check
            }
        }
Ejemplo n.º 10
0
        // This method will do the main data processing job.
        ModuleState::MODULE_EXITCODE Driver::body() {
                std::ofstream myFile("sensors.bin", ios::binary);
	        while (getModuleState() == ModuleState::RUNNING) {
                // In the following, you find example for the various data sources that are available:

		        // 1. Get most recent vehicle data:
		        Container containerVehicleData = getKeyValueDataStore().get(Container::VEHICLEDATA);
		        VehicleData vd = containerVehicleData.getData<VehicleData> ();
		 //       cerr << "Most recent vehicle data: '" << vd.toString() << "'" << endl;

		        // 2. Get most recent sensor board data:
		        Container containerSensorBoardData = getKeyValueDataStore().get(Container::USER_DATA_0);
		        SensorBoardData sbd = containerSensorBoardData.getData<SensorBoardData> ();
			
		      timeval time;
		      string test;
		      stringstream teststream;		      
		      
		      double val = sbd.getDistance(0);
		      myFile.write((char*)&val, sizeof(val));
		      
		      val = sbd.getDistance(1);
		      myFile.write((char*)&val, sizeof(val));
		      
		      val = sbd.getDistance(2);
		      myFile.write((char*)&val, sizeof(val));
		      
		      val = sbd.getDistance(3);
		      myFile.write((char*)&val, sizeof(val));
		      
		      val = sbd.getDistance(4);
		      myFile.write((char*)&val, sizeof(val));

		      gettimeofday(&time, NULL);
		      unsigned long millis = (time.tv_usec / 1000);
		      
		      teststream << millis;
		      if (millis < 10){
			test = remove_letter(currentDateTime(), ':') + "00" + teststream.str();
		      }
		      else if (millis < 100 and millis >= 10) {
			test = remove_letter(currentDateTime(), ':') + "0" + teststream.str();
			
		      }
		      else {
			test = remove_letter(currentDateTime(), ':') + teststream.str();
			
		      }
		      
		      
		      cerr << test << endl;
		      int timestamp;
		      timestamp = atoi(test.c_str());

		      myFile.write((char*)&timestamp, sizeof(timestamp));
			

		        // 3. Get most recent user button data:
		        Container containerUserButtonData = getKeyValueDataStore().get(Container::USER_BUTTON);
		        UserButtonData ubd = containerUserButtonData.getData<UserButtonData> ();
		  //      cerr << "Most recent user button data: '" << ubd.toString() << "'" << endl;

		        // 4. Get most recent steering data as fill from lanedetector for example:
		        Container containerSteeringData = getKeyValueDataStore().get(Container::USER_DATA_1);
		        SteeringData sd = containerSteeringData.getData<SteeringData> ();
		 //       cerr << "Most recent steering data: '" << sd.toString() << "'" << endl;



                // Design your control algorithm here depending on the input data from above.



		        // Create vehicle control data.
		        VehicleControl vc;

                // With setSpeed you can set a desired speed for the vehicle in the range of -2.0 (backwards) .. 0 (stop) .. +2.0 (forwards)
		        vc.setSpeed(1.5);

                // With setSteeringWheelAngle, you can steer in the range of -26 (left) .. 0 (straight) .. +25 (right)
                double desiredSteeringWheelAngle = 0; // 4 degree but SteeringWheelAngle expects the angle in radians!
		        vc.setSteeringWheelAngle(desiredSteeringWheelAngle * Constants::DEG2RAD);

                // You can also turn on or off various lights:
                vc.setBrakeLights(false);
                vc.setLeftFlashingLights(false);
                vc.setRightFlashingLights(true);

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