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; }
/* ----------------------------------- 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; }
// 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; }
// 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; }
// 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; } } }
// 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; }
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 } }
// 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*)×tamp, 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; }