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