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