void StreetMapMapWidget::nextContainer(Container &c) {
if (c.getDataType() == opendlv::data::environment::EgoState::ID()) {
EgoState es = c.getData<EgoState>();
cout << "[StreetMapMapWidget]: " << es.toString() << endl;
}
else {
cout << "[StreetMapMapWidget]: Received container " << c.getDataType() << endl;
}
}
coredata::dmcp::ModuleExitCodeMessage::ModuleExitCode DrivenPath::body() {
KeyValueDataStore &kvs = getKeyValueDataStore();
// Get scenario.
const URL urlOfSCNXFile(getKeyValueConfiguration().getValue<string>("global.scenario"));
// Create graph.
core::wrapper::graph::DirectedGraph m_graph;
// Read scenario.
vector<NamedLine> listOfLines;
if (urlOfSCNXFile.isValid()) {
SCNXArchive &scnxArchive = SCNXArchiveFactory::getInstance().getSCNXArchive(urlOfSCNXFile);
hesperia::data::scenario::Scenario &scenario = scnxArchive.getScenario();
// Construct road network.
LaneVisitor lv(m_graph, scenario);
scenario.accept(lv);
listOfLines = lv.getListOfLines();
}
vector<NamedLine>::const_iterator it = listOfLines.begin();
while (it != listOfLines.end()) {
NamedLine l =(*it++);
// Draw line.
Container c = Container(Container::DRAW_LINE, l);
getConference().send(c);
/*
cout << "Lines;" << l.getName() << ";" << l.getA().getX()
<< ";" << l.getA().getY()
<< ";" << l.getA().getZ()
<< ";" << l.getB().getX()
<< ";" << l.getB().getY()
<< ";" << l.getB().getZ() << endl;
*/
cout << "LinesA;" << l.getName() << ";" << l.getA().getX()
<< ";" << l.getA().getY()
<< ";" << l.getA().getZ() << endl;
cout << "LinesB;" << l.getName() << ";" << l.getB().getX()
<< ";" << l.getB().getY()
<< ";" << l.getB().getZ() << endl;
}
unsigned int counter = 0;
while (getModuleStateAndWaitForRemainingTimeInTimeslice() == coredata::dmcp::ModuleStateMessage::RUNNING) {
// Get current ego state.
Container c = kvs.get(Container::EGOSTATE);
EgoState es = c.getData<EgoState>();
cout << counter << ";EgoState: '" << es.toString() << "'" << endl;
cout << counter << ";EgoState-short;" << es.getPosition().getX() << ";" << es.getPosition().getY() << ";" << es.getPosition().getZ() << endl;
// Algorithm:
// 1. Get position of vehicle.
// 2. Calculate perpendicular point to all lines in the list.
// 3. Determine the distance between the position and the perpendicular point.
// 4. Iterate through all named lines and determine the smallest distance.
// 5. Print the smallest distance/named line.
string oldName = "";
double oldDistance = numeric_limits<double>::max();
string oldNameOverall = "";
double oldDistanceOverall = numeric_limits<double>::max();
it = listOfLines.begin();
while (it != listOfLines.end()) {
NamedLine l =(*it++);
if (l.getName().compare(oldName) != 0) {
// Print results:
if (oldName.compare("") != 0) {
cout << counter << ";Distance;" << oldName << ";" << oldDistance << endl;
}
// Reset values because we are starting a new turn.
oldDistance = numeric_limits<double>::max();
}
Point3 vehiclePosition = es.getPosition();
Point3 perpendicularPoint = l.getPerpendicularPoint(vehiclePosition);
// Compute distance between vehicle's position and lane segment.
double length = (perpendicularPoint - vehiclePosition).lengthXY();
if (length < oldDistance) {
oldDistance = length;
}
if (length < oldDistanceOverall) {
oldDistanceOverall = length;
oldNameOverall = l.getName();
}
oldName = l.getName();
}
cout << counter << ";DistanceOverall;" << oldNameOverall << ";" << oldDistanceOverall << endl;
counter++;
}
return coredata::dmcp::ModuleExitCodeMessage::OKAY;
}
void Data2StringStream::toStringStream(const EgoState &es) {
m_sstr << es.toString();
}
void DistanceToObjectsReport::report(const odcore::wrapper::Time &t) {
cerr << "Call to DistanceToObjectsReport for t = " << t.getSeconds() << "." << t.getPartialMicroseconds() << ", containing " << getFIFO().getSize() << " containers." << endl;
// Get last EgoState.
KeyValueDataStore &kvds = getKeyValueDataStore();
Container c = kvds.get(opendlv::data::environment::EgoState::ID());
EgoState es = c.getData<EgoState>();
const uint32_t SIZE = getFIFO().getSize();
for (uint32_t i = 0; i < SIZE; i++) {
c = getFIFO().leave();
cerr << "Received: " << c.toString() << endl;
if (c.getDataType() == opendlv::data::environment::Obstacle::ID()) {
Obstacle o = c.getData<Obstacle>();
const float DISTANCE = (es.getPosition().getDistanceTo(o.getPosition()));
cerr << "DistanceToObjectsReport: Distance to object: " << DISTANCE << ", E: " << es.toString() << ", o: " << o.getPosition().toString() << endl;
// Continuously check distance.
m_correctDistance &= (DISTANCE > m_threshold);
vector<Point3> shape = o.getPolygon().getVertices();
Point3 head = shape.front();
shape.push_back(head);
const uint32_t NUMVERTICES = shape.size();
for(uint32_t j = 1; j < NUMVERTICES; j++) {
Point3 pA = shape.at(j-1);
Point3 pB = shape.at(j);
// TODO: Check polygonal data as well as perpendicular to all sides.
// Create line.
Line l(pA, pB);
// Compute perpendicular point.
Point3 perpendicularPoint = l.getPerpendicularPoint(es.getPosition());
// Compute distance between current position and perpendicular point.
const float DISTANCE_PP = (es.getPosition().getDistanceTo(perpendicularPoint));
cerr << "DistanceToObjectsReport: Distance to object's shape: " << DISTANCE_PP << ", E: " << es.toString() << ", o: " << o.getPosition().toString() << ", perpendicular point:" << perpendicularPoint.toString() << endl;
// Continuously check distance.
m_correctDistance &= (DISTANCE > m_threshold);
}
}
if (c.getDataType() == opendlv::data::environment::OtherVehicleState::ID()) {
OtherVehicleState o = c.getData<OtherVehicleState>();
const float DISTANCE = (es.getPosition().getDistanceTo(o.getPosition()));
// Compute distance between current position and perpendicular point.
cerr << "DistanceToObjectsReport: Distance to other vehicle: " << DISTANCE << ", E: " << es.toString() << ", o: " << o.getPosition().toString() << endl;
// Continuously check distance.
m_correctDistance &= (DISTANCE > m_threshold);
}
}
}
