void ProxyIMU::setUp() {
odcore::base::KeyValueConfiguration kv = getKeyValueConfiguration();
double roll = kv.getValue<double>("proxy-imu.mount.roll")*M_PI/180.0;
double pitch = kv.getValue<double>("proxy-imu.mount.pitch")*M_PI/180.0;
double yaw = kv.getValue<double>("proxy-imu.mount.yaw")*M_PI/180.0;
std::vector<double> const mountRotation({roll, pitch, yaw});
std::string const type = kv.getValue<std::string>("proxy-imu.type");
uint32_t const calibrationNumberOfSamples = kv.getValue<uint32_t>("proxy-imu.calibration_number_of_samples");
bool const lockCalibration = (kv.getValue< int32_t >("proxy-imu.lockcalibration") == 1);
m_debug = (kv.getValue< int32_t >("proxy-imu.debug") == 1);
std::string const calibrationFile = kv.getValue<std::string>("proxy-imu.calibration_file");
if (type.compare("pololu.altimu10") == 0) {
std::string const deviceNode = kv.getValue< std::string >("proxy-imu.pololu.altimu10.device_node");
std::string const addressType = kv.getValue<std::string>("proxy-imu.pololu.altimu10.address_type");
if(addressType.compare("high") || addressType.compare("low")) {
m_device = std::unique_ptr<PololuAltImu10Device>(new PololuAltImu10Device(deviceNode, addressType, mountRotation, calibrationNumberOfSamples, calibrationFile, lockCalibration, m_debug));
} else {
std::cerr << "[proxy-imu] Address type invalid. Must be either high xor low." << std::endl;
}
std::cout << "[proxy-imu] Successfully initiated.";
}
if (m_device.get() == nullptr) {
std::cerr << "[proxy-imu] No valid device driver defined."
<< std::endl;
}
}
void OpticalFlow::setUp()
{
odcore::base::KeyValueConfiguration kv = getKeyValueConfiguration();
uint32_t termCritMaxCount = kv.getValue<uint32_t>(
"sensation-vision-opticalflow.termCritMaxCount");
double termCritEpsilon = kv.getValue<double>(
"sensation-vision-opticalflow.termCritEpsilon");
m_termcrit = cv::TermCriteria(cv::TermCriteria::COUNT|cv::TermCriteria::EPS,
termCritMaxCount,termCritEpsilon);
uint32_t lkSearchWindowWidth = kv.getValue<uint32_t>(
"sensation-vision-opticalflow.lkSearchWindowWidth");
m_searchSize = cv::Size(lkSearchWindowWidth,lkSearchWindowWidth);
m_maxLevel = kv.getValue<uint32_t>(
"sensation-vision-opticalflow.maxLevel");
m_minEigThreshold = kv.getValue<double>(
"sensation-vision-opticalflow.minEigThreshold");
m_nAxisPoints = kv.getValue<uint32_t>(
"sensation-vision-opticalflow.nAxisPoints");
m_name = "opticalflow";
m_size = (m_nAxisPoints)*(m_nAxisPoints)*3;
m_sharedMemory =
odcore::wrapper::SharedMemoryFactory::createSharedMemory(m_name, m_size);
m_outputSharedImage.setName(m_name);
m_outputSharedImage.setWidth(m_nAxisPoints);
m_outputSharedImage.setHeight(m_nAxisPoints);
m_outputSharedImage.setBytesPerPixel(3);
m_outputSharedImage.setSize(m_size);
m_flow = cv::Mat(m_nAxisPoints,m_nAxisPoints,CV_8UC3, cv::Scalar(0,0,0));
}
void Ivrule::setUp()
{
odcore::base::KeyValueConfiguration kv = getKeyValueConfiguration();
m_mioAngleRange = static_cast<float>(kv.getValue<double>("knowledge-ivrule.mioAngleRange")*opendlv::Constants::DEG2RAD);
m_mioDistanceRange = kv.getValue<float>("knowledge-ivrule.mioDistanceRange");
m_memoryDuration = kv.getValue<int32_t>("knowledge-ivrule.memoryDuration");
m_desiredAngularSize = static_cast<float>(kv.getValue<double>("knowledge-ivrule.desiredAngularSize")*opendlv::Constants::DEG2RAD);
m_desiredOpticalFlow = kv.getValue<float>("knowledge-ivrule.desiredOpticalFlow");
m_initialised = true;
std::cout << "Setup complete." << std::endl;
}
void CheckActuation::setUp()
{
odcore::base::KeyValueConfiguration kv = getKeyValueConfiguration();
m_steeringLimit = kv.getValue<float>(
"safety-checkactuation.steeringLimit");
m_accMaxLimit = kv.getValue<float>(
"safety-checkactuation.accMaxLimit");
m_maxAllowedDeceleration = kv.getValue<float>(
"safety-checkactuation.maxAllowedDeceleration");
m_initialised = true;
}
void Identity::setUp()
{
odcore::base::KeyValueConfiguration kv = getKeyValueConfiguration();
m_stationId = kv.getValue<uint32_t>("knowledge-identity.stationId");
m_stationType = kv.getValue<uint32_t>("knowledge-identity.stationType");
m_vehicleLength = kv.getValue<double>("knowledge-identity.vehicleLength");
m_vehicleWidth = kv.getValue<double>("knowledge-identity.vehicleWidth");
m_vehicleRole = kv.getValue<uint32_t>("knowledge-identity.vehicleRole");
m_rearAxleLocation = kv.getValue<double>("knowledge-identity.rearAxleLocation");
}
void Propulsion::setUp()
{
odcore::base::KeyValueConfiguration kv = getKeyValueConfiguration();
std::string const type =
kv.getValue<std::string>("proxy-actuator-propulsion.type");
if (type.compare("gw-volvo") == 0) {
// m_device = std::unique_ptr<Device>(new GatewayVolvoDevice());
}
if (m_device.get() == nullptr) {
std::cerr << "[proxy-actuator-propulsion] No valid device driver defined."
<< std::endl;
}
}
void SonarArray::setUp()
{
odcore::base::KeyValueConfiguration kv = getKeyValueConfiguration();
std::string const type =
kv.getValue<std::string>("proxy-sonararray.type");
if (type.compare("maxsonar") == 0) {
// m_device = std::unique_ptr<Device>(new MaxSonarDevice());
}
if (m_device.get() == nullptr) {
std::cerr << "[proxy-sonararray] No valid device driver defined."
<< std::endl;
}
}
void V2v::setUp()
{
odcore::base::KeyValueConfiguration kv = getKeyValueConfiguration();
std::string const type = kv.getValue<std::string>("proxy-sensor-v2v.type");
/* std::string const port =
kv.getValue<std::string>("proxy-sensor-v2v.port");
float const mountX = kv.getValue<float>("proxy-sensor-v2v.mount.x");
float const mountY = kv.getValue<float>("proxy-sensor-v2v.mount.y");
float const mountZ = kv.getValue<float>("proxy-sensor-v2v.mount.z");
*/
if (type.compare("geonetworking-dual") == 0) {
// m_device = std::unique_ptr<Device>(new GeonetworkingDualDevice());
}
if (m_device.get() == nullptr) {
std::cerr << "[proxy-sensor-v2v] No valid device driver defined."
<< std::endl;
}
}
void Lidar::setUp()
{
odcore::base::KeyValueConfiguration kv = getKeyValueConfiguration();
std::string const type = kv.getValue<std::string>("proxy-lidar.type");
/* std::string const port =
kv.getValue<std::string>("proxy-lidar.port");
float const mountX = kv.getValue<float>("proxy-lidar.mount.x");
float const mountY = kv.getValue<float>("proxy-lidar.mount.y");
float const mountZ = kv.getValue<float>("proxy-lidar.mount.z");
*/
if (type.compare("sick") == 0) {
// m_device = std::unique_ptr<Device>(new SickDevice());
}
if (m_device.get() == nullptr) {
std::cerr << "[proxy-lidar] No valid device driver defined."
<< std::endl;
}
}
void Relay::setUp()
{
odcore::base::KeyValueConfiguration kv = getKeyValueConfiguration();
std::string const type = kv.getValue<std::string>("proxy-relay.type");
std::string const valuesString =
kv.getValue<std::string>("proxy-relay.values");
std::vector<bool> values;
std::vector<std::string> valuesVector =
odcore::strings::StringToolbox::split(valuesString, ',');
for (auto valueString : valuesVector) {
bool value = static_cast<bool>(std::stoi(valueString));
values.push_back(value);
}
if (type.compare("gpio") == 0) {
std::string const pinsString =
kv.getValue<std::string>("proxy-relay.gpio.pins");
std::vector<uint16_t> pins;
std::vector<std::string> pinsVector =
odcore::strings::StringToolbox::split(pinsString, ',');
for (auto pinString : pinsVector) {
uint16_t pin = std::stoi(pinString);
pins.push_back(pin);
}
m_device = std::unique_ptr<Device>(new GpioDevice(values, pins));
}
if (m_device.get() == nullptr) {
std::cerr << "[proxy-relay] No valid device driver defined."
<< std::endl;
}
}
void ConfigurationViewerPlugIn::setupPlugin() {
m_viewerWidget = new ConfigurationViewerWidget(*this, getKeyValueConfiguration(), getParentQWidget());
}
void SonarArray::setUp()
{
odcore::base::KeyValueConfiguration kv = getKeyValueConfiguration();
std::string const type = kv.getValue<std::string>("proxy-sonararray.type");
std::string const xsString =
kv.getValue<std::string>("proxy-sonararray.mount.x");
std::vector<std::string> xsVector =
odcore::strings::StringToolbox::split(xsString, ',');
std::string const ysString =
kv.getValue<std::string>("proxy-sonararray.mount.y");
std::vector<std::string> ysVector =
odcore::strings::StringToolbox::split(ysString, ',');
std::string const zsString =
kv.getValue<std::string>("proxy-sonararray.mount.z");
std::vector<std::string> zsVector =
odcore::strings::StringToolbox::split(zsString, ',');
std::string const azimuthsString =
kv.getValue<std::string>("proxy-sonararray.mount.azimuth");
std::vector<std::string> azimuthsVector =
odcore::strings::StringToolbox::split(azimuthsString, ',');
std::string const zenithsString =
kv.getValue<std::string>("proxy-sonararray.mount.zenith");
std::vector<std::string> zenithsVector =
odcore::strings::StringToolbox::split(zenithsString, ',');
uint16_t length = xsVector.size();
if (length != ysVector.size() || length != zsVector.size()
|| length != azimuthsVector.size() || length != zenithsVector.size()) {
std::cerr << "[proxy-sonararray] Vector size mismatch." << std::endl;
}
std::vector<opendlv::model::Cartesian3> positions;
std::vector<opendlv::model::Direction> directions;
for (uint16_t i = 0; i < length; i++) {
float x = std::stof(xsVector[i]);
float y = std::stof(ysVector[i]);
float z = std::stof(zsVector[i]);
opendlv::model::Cartesian3 position(x, y, z);
positions.push_back(position);
float azimuth = std::stof(azimuthsVector[i]);
float zenith = std::stof(zenithsVector[i]);
opendlv::model::Direction direction(azimuth, zenith);
directions.push_back(direction);
}
if (type.compare("max") == 0) {
std::vector<uint16_t> pins;
std::string const pinsString =
kv.getValue<std::string>("proxy-sonararray.max.pin");
std::vector<std::string> pinsVector =
odcore::strings::StringToolbox::split(pinsString, ',');
for (auto pinString : pinsVector) {
uint16_t pin = std::stoi(pinString);
pins.push_back(pin);
}
float scaleValue = kv.getValue<float>("proxy-sonararray.max.scale_value");
m_device = std::unique_ptr<Device>(new MaxDevice(positions, directions,
pins, scaleValue));
}
if (m_device.get() == nullptr) {
std::cerr << "[proxy-sonararray] No valid device driver defined."
<< std::endl;
}
}
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
}
}
