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