Esempio n. 1
0
    // This method will do the main data processing job.
    ModuleState::MODULE_EXITCODE Proxy::body() {

        uint32_t captureCounter = 0;
        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: '" << vc.getSpeed() << "'" << endl;
                cerr << "Angle: '" << vc.getSteeringWheelAngle() << "'" << endl;

            // TODO: Here, you need to implement the data links to the embedded system
            // to read data from IR/US.

            // Test ***************************
            // Markus Erlach
            string in = "";
            string rec;
            char command[10];
            cout << "Enter command to send, " << endl;
            cout << "Command alternatives: w, f, s, r, n, h, v, m" << endl;
            cin >> in;
            /* w, f, s, r, n, h, v
            w = set speed to 1560, f = accelerate by 10
            s = slow down, r = reverse, n = neutral
            h = turn right, v = turn left    
            */
            strcpy(command, in.c_str());            
            write(port, command, 10);
            cout << "Proxy2 wrote: "<< command << endl;
            rec = msv::readSerial();
            decode(rec);

            Container c = Container(Container::USER_DATA_0, sensorBoardData);
            distribute(c);

            /*int IR1Data = sensorBoardData.getValueForKey_MapOfDistances(0);
            cout << "SBD IR1: " << IR1Data << endl;
            */
            //flushes the input queue, which contains data that have been received but not yet read.
            tcflush(port, TCIFLUSH); 
           
        }
        cout << "Proxy: Captured " << captureCounter << " frames." << endl;

        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;
    }
Esempio n. 3
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;
    }
        // This method will do the main data processing job.
        odcore::data::dmcp::ModuleExitCodeMessage::ModuleExitCode Proxy::body() {
            uint32_t captureCounter = 0;
            uint32_t sendCounter = 0; // Make sure that we don't send every loop
            uint32_t delayCounter = 0;  // Delay the send over serial on startup
            uint32_t portNumber = 0;
            //int16_t oldSpeed = 1500;
            //int16_t oldAngle = 90;
            size_t readSize = 1;  // Size of data to read from the serial port
            bool newCommand = false;
            string port = "/dev/ttyACM";
            unsigned long baud = 57600;
            string result = ""; // Incoming serial buffer
            bool serialOpen = false;

            // Try opening the serial connections, tries ports 0-4 automatically
      			while(!serialOpen && portNumber < 4){
      				string xport = port + to_string(portNumber);
      				try {
                my_serial = unique_ptr<serial::Serial>(new Serial(xport, baud, serial::Timeout::simpleTimeout(1000)));
                serialOpen = true;
                cout << "Trying port " << xport << endl;
              } catch (IOException e){
                cerr << "Error setting up serialport: " << xport << endl;
                serialOpen = false;
              }
              // Test if the serial port has been created correctly.
              cout << "Is the serial port open?";
              if(my_serial->isOpen()){
                cout << " Yes." << endl;
              } else {
                cout << " No." << endl;
              }
      				portNumber ++;
      			}

            while (getModuleStateAndWaitForRemainingTimeInTimeslice() == odcore::data::dmcp::ModuleStateMessage::RUNNING) {
                // Delay all communication for a short while to not overflow the arduino
                if (delayCounter < 40){
                  delayCounter++;
                } else {
                  // Capture frame.
  		            if (m_camera.get() != NULL) {
  		                odcore::data::image::SharedImage si = m_camera->capture();

  		                Container c(si);
  		                distribute(c);
  		                captureCounter++;
  		            }

                  // Create objects where we get speed and steering angle from
  		            Container containerVehicleControl = getKeyValueDataStore().get(automotive::VehicleControl::ID());
  		            VehicleControl vc = containerVehicleControl.getData<VehicleControl>();

  		            double vcSpeed = vc.getSpeed();
  		            int16_t speedTemp = vcSpeed;
                  // Set the speed to constant values
  		            if (vcSpeed > 0){
  		              speedTemp = 1615;
  		            } else if (vcSpeed < 0){
  		              speedTemp = 1070;
  		            } else {
  		              speedTemp = 1500;
  		            }
  		            //vcSpeed = speedTemp;

  		            double vcAngle = vc.getSteeringWheelAngle();
                  // Convert steering angle from radiants to degrees
  		            int16_t vcDegree = (vcAngle * cartesian::Constants::RAD2DEG);
                  // Send the exact value we want to set steering at
  		            if (vcDegree > 25){
  		              vcDegree = 90 + 40;
  		            } else if (vcDegree < -25){
  		              vcDegree = 90 - 30;
  		            } else {
  		              vcDegree = 90 + vcDegree;
  		            }

  		            sendCounter++;  // Dont send to arduino every loop
  		            //if(my_serial->isOpen() && sendCounter > 5 && (oldSpeed != speedTemp || oldAngle != vcDegree)){
  		            if(my_serial->isOpen() && sendCounter > 5){
                    sendOverSerial(speedTemp, vcDegree);
  		              sendCounter = 0;
  		             //oldSpeed = speedTemp;
  		             //oldAngle = vcDegree;
  		            }
                  // Read to a buffer for incoming data
  		            while (my_serial->available() > 0){
  		              string c = my_serial->read(readSize);
  		              result += c;
  		              if (c == ","){
  		                newCommand = true; // When a full netstring has arrived
  		              }
  		            }
  		            if (newCommand){
  		              string decoded = netstrings.decodeNetstring(result);
  		              sbdDistribute(decoded); // Distribute SensorBoardData
                    vdDistribute(decoded); // Distribute VehicleData
  		              newCommand = false;
  		              result = ""; // clear buffer
  		            }
		            }
		     }

		        cout << "Proxy: Captured " << captureCounter << " frames." << endl;

		        return odcore::data::dmcp::ModuleExitCodeMessage::OKAY;
        }