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