// This method will do the main data processing job.
ModuleState::MODULE_EXITCODE Proxy::body() {
uint32_t captureCounter = 0;
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: '" << vc.getSpeed() << "'" << endl;
cerr << "Angle: '" << vc.getSteeringWheelAngle() << "'" << endl;
// TODO: Here, you need to implement the data links to the embedded system
// to read data from IR/US.
// Test ***************************
// Markus Erlach
string in = "";
string rec;
char command[10];
cout << "Enter command to send, " << endl;
cout << "Command alternatives: w, f, s, r, n, h, v, m" << endl;
cin >> in;
/* w, f, s, r, n, h, v
w = set speed to 1560, f = accelerate by 10
s = slow down, r = reverse, n = neutral
h = turn right, v = turn left
*/
strcpy(command, in.c_str());
write(port, command, 10);
cout << "Proxy2 wrote: "<< command << endl;
rec = msv::readSerial();
decode(rec);
Container c = Container(Container::USER_DATA_0, sensorBoardData);
distribute(c);
/*int IR1Data = sensorBoardData.getValueForKey_MapOfDistances(0);
cout << "SBD IR1: " << IR1Data << endl;
*/
//flushes the input queue, which contains data that have been received but not yet read.
tcflush(port, TCIFLUSH);
}
cout << "Proxy: Captured " << captureCounter << " frames." << endl;
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;
}
// 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;
}
// This method will do the main data processing job.
odcore::data::dmcp::ModuleExitCodeMessage::ModuleExitCode Proxy::body() {
uint32_t captureCounter = 0;
uint32_t sendCounter = 0; // Make sure that we don't send every loop
uint32_t delayCounter = 0; // Delay the send over serial on startup
uint32_t portNumber = 0;
//int16_t oldSpeed = 1500;
//int16_t oldAngle = 90;
size_t readSize = 1; // Size of data to read from the serial port
bool newCommand = false;
string port = "/dev/ttyACM";
unsigned long baud = 57600;
string result = ""; // Incoming serial buffer
bool serialOpen = false;
// Try opening the serial connections, tries ports 0-4 automatically
while(!serialOpen && portNumber < 4){
string xport = port + to_string(portNumber);
try {
my_serial = unique_ptr<serial::Serial>(new Serial(xport, baud, serial::Timeout::simpleTimeout(1000)));
serialOpen = true;
cout << "Trying port " << xport << endl;
} catch (IOException e){
cerr << "Error setting up serialport: " << xport << endl;
serialOpen = false;
}
// Test if the serial port has been created correctly.
cout << "Is the serial port open?";
if(my_serial->isOpen()){
cout << " Yes." << endl;
} else {
cout << " No." << endl;
}
portNumber ++;
}
while (getModuleStateAndWaitForRemainingTimeInTimeslice() == odcore::data::dmcp::ModuleStateMessage::RUNNING) {
// Delay all communication for a short while to not overflow the arduino
if (delayCounter < 40){
delayCounter++;
} else {
// Capture frame.
if (m_camera.get() != NULL) {
odcore::data::image::SharedImage si = m_camera->capture();
Container c(si);
distribute(c);
captureCounter++;
}
// Create objects where we get speed and steering angle from
Container containerVehicleControl = getKeyValueDataStore().get(automotive::VehicleControl::ID());
VehicleControl vc = containerVehicleControl.getData<VehicleControl>();
double vcSpeed = vc.getSpeed();
int16_t speedTemp = vcSpeed;
// Set the speed to constant values
if (vcSpeed > 0){
speedTemp = 1615;
} else if (vcSpeed < 0){
speedTemp = 1070;
} else {
speedTemp = 1500;
}
//vcSpeed = speedTemp;
double vcAngle = vc.getSteeringWheelAngle();
// Convert steering angle from radiants to degrees
int16_t vcDegree = (vcAngle * cartesian::Constants::RAD2DEG);
// Send the exact value we want to set steering at
if (vcDegree > 25){
vcDegree = 90 + 40;
} else if (vcDegree < -25){
vcDegree = 90 - 30;
} else {
vcDegree = 90 + vcDegree;
}
sendCounter++; // Dont send to arduino every loop
//if(my_serial->isOpen() && sendCounter > 5 && (oldSpeed != speedTemp || oldAngle != vcDegree)){
if(my_serial->isOpen() && sendCounter > 5){
sendOverSerial(speedTemp, vcDegree);
sendCounter = 0;
//oldSpeed = speedTemp;
//oldAngle = vcDegree;
}
// Read to a buffer for incoming data
while (my_serial->available() > 0){
string c = my_serial->read(readSize);
result += c;
if (c == ","){
newCommand = true; // When a full netstring has arrived
}
}
if (newCommand){
string decoded = netstrings.decodeNetstring(result);
sbdDistribute(decoded); // Distribute SensorBoardData
vdDistribute(decoded); // Distribute VehicleData
newCommand = false;
result = ""; // clear buffer
}
}
}
cout << "Proxy: Captured " << captureCounter << " frames." << endl;
return odcore::data::dmcp::ModuleExitCodeMessage::OKAY;
}
