odcore::data::dmcp::ModuleExitCodeMessage::ModuleExitCode ProxyIMU::body() {
while (getModuleStateAndWaitForRemainingTimeInTimeslice() == odcore::data::dmcp::ModuleStateMessage::RUNNING) {
opendlv::proxy::AngularVelocityReading gyroscopeReading = m_device->ReadGyroscope();
odcore::data::Container gyroscopeContainer(gyroscopeReading);
getConference().send(gyroscopeContainer);
opendlv::proxy::AccelerationReading accelerometerReading = m_device->ReadAccelerometer();
odcore::data::Container accelerometerContainer(accelerometerReading);
getConference().send(accelerometerContainer);
opendlv::proxy::MagneticFieldReading magnetometerReading = m_device->ReadMagnetometer();
odcore::data::Container magnetometerContainer(magnetometerReading);
getConference().send(magnetometerContainer);
opendlv::proxy::AltitudeReading altimeterReading = m_device->ReadAltimeter();
odcore::data::Container altimeterContainer(altimeterReading);
getConference().send(altimeterContainer);
opendlv::proxy::TemperatureReading temperatureReading = m_device->ReadTemperature();
odcore::data::Container temperatureContainer(temperatureReading);
getConference().send(temperatureContainer);
if (m_debug) {
std::cout << gyroscopeReading.toString() << ", "
<< accelerometerReading.toString() << ", "
<< magnetometerReading.toString() << ", "
<< altimeterReading.toString() << ", "
<< temperatureReading.toString() << std::endl;
}
}
return odcore::data::dmcp::ModuleExitCodeMessage::OKAY;
}
void Identity::SendContainer(opendlv::knowledge::Insight &a_insight)
{
odcore::data::Container c(a_insight);
getConference().send(c);
// Allow sending packets.
odcore::base::Thread::usleepFor(100);
// Resend to v2vcam
odcore::data::Container c2(a_insight, opendlv::knowledge::Insight::ID() + 400);
getConference().send(c2);
}
void DirectionOfMovement::SendContainer()
{
opendlv::model::Direction direction(0.0f, 0.0f);
opendlv::sensation::DirectionOfMovement nextMessage(direction);
odcore::data::Container c(nextMessage);
getConference().send(c);
}
/**
* Receives raw vehicle message from proxy.
* Sends raw vehicle message to linguistics handlers.
*/
void Audition::nextContainer(odcore::data::Container &c)
{
// cout << "Received container of type " << c.getDataType()
// << " sent at " << c.getSentTimeStamp().getYYYYMMDD_HHMMSSms()
// <<" received at " << c.getReceivedTimeStamp().getYYYYMMDD_HHMMSSms()
// << endl;
if(c.getDataType() == opendlv::proxy::V2vInbound::ID()){
// std::cout << "Received a message of type ."
opendlv::proxy::V2vInbound message = c.getData<opendlv::proxy::V2vInbound>();
std::string dataString = message.getData();
std::vector<unsigned char> data(dataString.begin(), dataString.end());
// string value = message.getValue();
// vector<string> tokens = odcore::strings::StringToolbox::split(value, '|');
// std::cout << std::to_string(static_cast<unsigned char>(*data.begin()))
// << std::endl;
// std::vector<unsigned char> const bytes = data;
// std::stringstream ss;
// for (uint i = 0; i < bytes.size(); i++) {
// ss << std::to_string(bytes.at(i));
// ss << "|";
// }
// std::cout<<ss.str()<<std::endl;
// std::cout<<value<<std::endl;
unsigned char v2vMsgId = data.at(0);
// std::cout << std::to_string(v2vMsgId)<<std::endl;
std::string v2vMsgType;
switch(v2vMsgId)
{
case 1:
v2vMsgType = "denm";
break;
case 2:
v2vMsgType = "cam";
break;
case 10:
v2vMsgType = "iclcm";
break;
default:
std::cout << "Received invalid message ID.";
}
if(!v2vMsgType.empty())
{
// std::cout<<"Sorted and sending to next layer.\n";
opendlv::sensation::Voice nextMessage(v2vMsgType, message.getSize(), dataString);
odcore::data::Container container(nextMessage);
getConference().send(container);
}
}
}
void Proxy::distribute(Container c) {
// Store data to recorder.
if (m_recorder != NULL) {
// Time stamp data before storing.
c.setReceivedTimeStamp(TimeStamp());
m_recorder->store(c);
}
// Share data.
getConference().send(c);
}
odcore::data::dmcp::ModuleExitCodeMessage::ModuleExitCode SonarArray::body()
{
while (getModuleStateAndWaitForRemainingTimeInTimeslice()
== odcore::data::dmcp::ModuleStateMessage::RUNNING) {
auto sonarReadings = m_device->GetEchoReadings();
odcore::data::Container sonarReadingContainer(sonarReadings);
getConference().send(sonarReadingContainer);
}
return odcore::data::dmcp::ModuleExitCodeMessage::OKAY;
}
odcore::data::dmcp::ModuleExitCodeMessage::ModuleExitCode Ivrule::body()
{
while (getModuleStateAndWaitForRemainingTimeInTimeslice() == odcore::data::dmcp::ModuleStateMessage::RUNNING) {
odcore::data::TimeStamp now;
if((m_mioValidUntil-now).toMicroseconds() > 0) {
// Steer to mio
opendlv::perception::StimulusDirectionOfMovement sdom(now, m_mio.getDirection(),opendlv::model::Direction(0,0));
odcore::data::Container containerSdom(sdom);
getConference().send(containerSdom);
// std::cout << "Sent sdom." << std::endl;
opendlv::perception::StimulusAngularSizeAlignment sasa(now, m_mio.getDirection(),m_mio.getAngularSize(),m_desiredAngularSize);
odcore::data::Container containerSasa(sasa);
getConference().send(containerSasa);
// std::cout << "Sent sasa." << std::endl;
// std::cout
// << " Id: " << m_mio.getObjectId()
// << " Azimuth: " << m_mio.getDirection().getAzimuth()
// << " Distance: " << m_mio.getDistance()
// << " Desired angular size: " << m_desiredAngularSize
// << " Currently: " << m_mio.getAngularSize()
// << std::endl;
} else {
opendlv::perception::StimulusGroundSpeed sof(now, m_desiredOpticalFlow, m_speed);
odcore::data::Container containerSof(sof);
getConference().send(containerSof);
// std::cout << "Send sof. Desired OF: " << m_desiredOpticalFlow << " Current OF: " << m_speed << std::endl;
}
}
return odcore::data::dmcp::ModuleExitCodeMessage::OKAY;
}
void OpticalFlow::SendContainer()
{
uint16_t numberOfPoints = m_staticImagePoints.size();
std::vector<opendlv::model::Direction> directions;
std::vector<float> u;
std::vector<float> v;
for(uint8_t i = 0; i < m_staticImagePoints.size(); i++){
// std::cout<< m_staticImagePoints[i].x << std::endl;
float x = m_staticImagePoints[i].x;
float y = m_staticImagePoints[i].y;
opendlv::model::Direction direction(x, y);
directions.push_back(direction);
u.push_back(m_endImagePoints[i].x - x);
v.push_back(m_endImagePoints[i].y - y);
}
opendlv::sensation::OpticalFlow nextMessage(numberOfPoints, directions, u, v);
odcore::data::Container c(nextMessage);
getConference().send(c);
}
void CheckActuation::nextContainer(odcore::data::Container &a_container)
{
if (a_container.getDataType() == opendlv::proxy::ActuationRequest::ID()+300){
opendlv::proxy::ActuationRequest actuationRequest
= a_container.getData<opendlv::proxy::ActuationRequest>();
float acceleration = actuationRequest.getAcceleration();
float steering = actuationRequest.getSteering();
// clamp steering
if (steering < -m_steeringLimit) {
steering = -m_steeringLimit;
std::cout << "steering request was capped to " << steering << std::endl;
}
else if (steering > m_steeringLimit) {
steering = m_steeringLimit;
std::cout << "steering request was capped to " << steering << std::endl;
}
// clamp acceleration
if (acceleration < -m_maxAllowedDeceleration) {
acceleration = -m_maxAllowedDeceleration;
std::cout << "acceleration request was capped to " << acceleration << std::endl;
}
else if (acceleration > m_accMaxLimit) {
acceleration = m_accMaxLimit;
std::cout << "acceleration request was capped to " << acceleration << std::endl;
}
actuationRequest.setAcceleration(acceleration);
actuationRequest.setSteering(steering);
actuationRequest.setIsValid(true);
odcore::data::Container c(actuationRequest);
getConference().send(c);
}
}
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
}
}
