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