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);
}
}
}
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;
}
}
vector<Container> IRUS::calculate(const EgoState &es) {
vector<Container> retVal;
// Store distance information.
automotive::miniature::SensorBoardData sensorBoardData;
// Loop through point sensors.
map<string, PointSensor*, odcore::strings::StringComparator>::iterator sensorIterator = m_mapOfPointSensors.begin();
for (; sensorIterator != m_mapOfPointSensors.end(); sensorIterator++) {
PointSensor *sensor = sensorIterator->second;
// Update FOV.
Polygon FOV = sensor->updateFOV(es.getPosition(), es.getRotation());
m_FOVs[sensor->getName()] = FOV;
// Calculate distance.
m_distances[sensor->getName()] = sensor->getDistance(m_mapOfPolygons);
cerr << sensor->getName() << ": " << m_distances[sensor->getName()] << endl;
// Store data for sensorboard.
sensorBoardData.putTo_MapOfDistances(sensor->getID(), m_distances[sensor->getName()]);
}
// Create a container with type automotive::miniature::SensorBoardData.
Container c = Container(sensorBoardData);
// Enqueue container.
retVal.push_back(c);
// Distribute FOV where necessary.
uint32_t sensorID = 9000;
map<string, Polygon, odcore::strings::StringComparator>::iterator FOVIterator = m_FOVs.begin();
for (; FOVIterator != m_FOVs.end(); FOVIterator++) {
string key = FOVIterator->first;
Polygon FOV = FOVIterator->second;
PointSensor *ps = m_mapOfPointSensors[key];
if ( (ps != NULL) && (ps->hasShowFOV()) ) {
// Send FOV.
Obstacle FOVobstacle(sensorID++, Obstacle::UPDATE);
FOVobstacle.setPolygon(FOV);
Container c2 = Container(FOVobstacle);
// Enqueue container.
retVal.push_back(c2);
}
}
return retVal;
}
void Renderer::process(Container &c) {
m_drawMap[c.getDataType()] = c;
// Get data for following the EgoState.
if (c.getDataType() == Container::EGOSTATE) {
EgoState egostate = c.getData<EgoState>();
Point3 dirCamera(-10, 0, 0);
dirCamera.rotateZ(egostate.getRotation().getAngleXY());
m_positionCamera.setX(egostate.getPosition().getX() + dirCamera.getX());
m_positionCamera.setY(egostate.getPosition().getY() + dirCamera.getY());
m_positionCamera.setZ(10);
m_lookAtPointCamera.setX(egostate.getPosition().getX());
m_lookAtPointCamera.setY(egostate.getPosition().getY());
m_lookAtPointCamera.setZ(0);
}
}
void DataRenderer::draw(const EgoState &es) {
Lock l(m_dataRendererMutex);
if (m_renderer != NULL) {
if (m_egoStateModel.size() > 0) {
// Use model of triangle sets for drawing.
m_renderer->beginPainting();
// TODO: Rotation for OpenGL is given in Eulerian angles for X-axis, for Y-axis, and for Z-axis SEPERATELY!
Point3 dir(0, 0, es.getRotation().getAngleXY());
m_renderer->drawListOfTriangleSets(m_egoStateModel, es.getPosition(), dir);
m_renderer->endPainting();
}
else {
// Use box.
Point3 p1(2, 1, 0);
Point3 p2(2, -1, 0);
Point3 p3(-2, -1, 0);
Point3 p4(-2, 1, 0);
p1.rotateZ(es.getRotation().getAngleXY());
p1 += es.getPosition();
p2.rotateZ(es.getRotation().getAngleXY());
p2 += es.getPosition();
p3.rotateZ(es.getRotation().getAngleXY());
p3 += es.getPosition();
p4.rotateZ(es.getRotation().getAngleXY());
p4 += es.getPosition();
vector<Point3> listOfPoints;
listOfPoints.push_back(p1);
listOfPoints.push_back(p2);
listOfPoints.push_back(p3);
listOfPoints.push_back(p4);
listOfPoints.push_back(p1);
m_renderer->beginPainting();
m_renderer->setColor(Point3(0.7, 0.7, 0.7));
m_renderer->setLineWidth(2.0);
m_renderer->drawPolyLine(listOfPoints, 1.0);
m_renderer->endPainting();
}
}
}
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;
}
ModuleState::MODULE_EXITCODE IRUS::body() {
// Load scenario.
const URL urlOfSCNXFile(getKeyValueConfiguration().getValue<string>("global.scenario"));
if (urlOfSCNXFile.isValid()) {
SCNXArchive &scnxArchive = SCNXArchiveFactory::getInstance().getSCNXArchive(urlOfSCNXFile);
hesperia::data::scenario::Scenario &scenario = scnxArchive.getScenario();
const hesperia::data::scenario::Surroundings &surroundings = scenario.getGround().getSurroundings();
const vector<hesperia::data::scenario::Shape*> &listOfShapes = surroundings.getListOfShapes();
vector<hesperia::data::scenario::Shape*>::const_iterator it = listOfShapes.begin();
while (it != listOfShapes.end()) {
hesperia::data::scenario::Shape *shape = (*it++);
if (shape != NULL) {
if (shape->getType() == hesperia::data::scenario::Shape::POLYGON) {
hesperia::data::scenario::Polygon *polygon = dynamic_cast<hesperia::data::scenario::Polygon*>(shape);
if (polygon != NULL) {
Polygon p;
m_numberOfPolygons++;
const vector<hesperia::data::scenario::Vertex3> &listOfVertices = polygon->getListOfVertices();
vector<hesperia::data::scenario::Vertex3>::const_iterator jt = listOfVertices.begin();
while (jt != listOfVertices.end()) {
p.add(*jt++);
}
m_mapOfPolygons[m_numberOfPolygons] = p;
}
}
}
}
}
// Show found polygons on console and in monitor.
const bool showPolygons = getKeyValueConfiguration().getValue<bool>("irus.showPolygons");
if (showPolygons) {
map<uint32_t, Polygon>::iterator it = m_mapOfPolygons.begin();
while (it != m_mapOfPolygons.end()) {
const uint32_t polygonID = it->first;
Polygon p = it->second;
Obstacle polygonObstacle(polygonID, Obstacle::UPDATE);
polygonObstacle.setPolygon(p);
// Send obstacle.
Container c = Container(Container::OBSTACLE, polygonObstacle);
getConference().send(c);
cerr << "Found polygon: " << it->second.toString() << endl;
it++;
}
}
// Setup all point sensors.
for (uint32_t i = 0; i < getKeyValueConfiguration().getValue<uint32_t>("irus.numberOfSensors"); i++) {
stringstream sensorID;
sensorID << "irus.sensor" << i << ".id";
uint16_t id(getKeyValueConfiguration().getValue<uint16_t>(sensorID.str()));
stringstream sensorName;
sensorName << "irus.sensor" << i << ".name";
string name(getKeyValueConfiguration().getValue<string>(sensorName.str()));
stringstream sensorTranslation;
sensorTranslation << "irus.sensor" << i << ".translation";
Point3 translation(getKeyValueConfiguration().getValue<string>(sensorTranslation.str()));
stringstream sensorRotZ;
sensorRotZ << "irus.sensor" << i << ".rotZ";
const double rotZ = getKeyValueConfiguration().getValue<double>(sensorRotZ.str());
stringstream sensorAngleFOV;
sensorAngleFOV << "irus.sensor" << i << ".angleFOV";
const double angleFOV = getKeyValueConfiguration().getValue<double>(sensorAngleFOV.str());
stringstream sensorDistanceFOV;
sensorDistanceFOV << "irus.sensor" << i << ".distanceFOV";
const double distanceFOV = getKeyValueConfiguration().getValue<double>(sensorDistanceFOV.str());
stringstream sensorClampDistance;
sensorClampDistance << "irus.sensor" << i << ".clampDistance";
const double clampDistance = getKeyValueConfiguration().getValue<double>(sensorClampDistance.str());
stringstream sensorShowFOV;
sensorShowFOV << "irus.sensor" << i << ".showFOV";
const bool showFOV = getKeyValueConfiguration().getValue<bool>(sensorShowFOV.str());
PointSensor *ps = new PointSensor(id, name, translation, rotZ, angleFOV, distanceFOV, clampDistance, showFOV);
if (ps != NULL) {
// Save for later.
m_mapOfPointSensors[ps->getName()] = ps;
// Initialize distance map entry.
m_distances[ps->getName()] = -1;
// Initialize FOV map entry.
Polygon f;
m_FOVs[ps->getName()] = f;
cout << "Registered point sensor " << ps->toString() << "." << endl;
}
}
// Use the most recent EgoState available.
KeyValueDataStore &kvs = getKeyValueDataStore();
// MSV: Variable for data from the sensorboard.
msv::SensorBoardData sensorBoardData;
// Loop through the map of polygons with the current EgoState and intersect all with the point sensor's FOV.
while (getModuleState() == ModuleState::RUNNING) {
// Get current EgoState.
Container c = kvs.get(Container::EGOSTATE);
EgoState es = c.getData<EgoState>();
// Loop through point sensors.
map<string, PointSensor*, core::wrapper::StringComparator>::iterator sensorIterator = m_mapOfPointSensors.begin();
for (; sensorIterator != m_mapOfPointSensors.end(); sensorIterator++) {
PointSensor *sensor = sensorIterator->second;
// Update FOV.
Polygon FOV = sensor->updateFOV(es.getPosition(), es.getRotation());
m_FOVs[sensor->getName()] = FOV;
// Calculate distance.
m_distances[sensor->getName()] = sensor->getDistance(m_mapOfPolygons);
cerr << sensor->getName() << ": " << m_distances[sensor->getName()] << endl;
// MSV: Store data for sensorboard.
sensorBoardData.update(sensor->getID(), m_distances[sensor->getName()]);
// MSV: Create a container with type USER_DATA_0.
c = Container(Container::USER_DATA_0, sensorBoardData);
// MSV: Send container.
getConference().send(c);
}
// Distribute FOV where necessary.
uint32_t sensorID = 9000;
map<string, Polygon, core::wrapper::StringComparator>::iterator FOVIterator = m_FOVs.begin();
for (; FOVIterator != m_FOVs.end(); FOVIterator++) {
string key = FOVIterator->first;
Polygon FOV = FOVIterator->second;
PointSensor *ps = m_mapOfPointSensors[key];
if ( (ps != NULL) && (ps->hasShowFOV()) ) {
// Send FOV.
Obstacle FOVobstacle(sensorID++, Obstacle::UPDATE);
FOVobstacle.setPolygon(FOV);
// Send obstacle.
c = Container(Container::OBSTACLE, FOVobstacle);
getConference().send(c);
}
}
}
// Delete all point sensors.
map<string, PointSensor*, core::wrapper::StringComparator>::const_iterator sensorIterator = m_mapOfPointSensors.begin();
for (; sensorIterator != m_mapOfPointSensors.end(); sensorIterator++) {
PointSensor *sensor = sensorIterator->second;
OPENDAVINCI_CORE_DELETE_POINTER(sensor);
}
m_mapOfPointSensors.clear();
return ModuleState::OKAY;
}
void Data2StringStream::toStringStream(const EgoState &es) {
m_sstr << es.toString();
}
void EnvironmentViewerGLWidget::nextContainer(Container &c) {
if (c.getDataType() == Container::EGOSTATE) {
m_numberOfReceivedEgoStates++;
if (m_egoStateNode != NULL) {
Lock l(m_rootMutex);
EgoState egostate = c.getData<EgoState>();
Point3 dir(0, 0, egostate.getRotation().getAngleXY());
m_egoStateNode->setRotation(dir);
m_egoStateNode->setTranslation(egostate.getPosition());
Position egoPosition;
egoPosition.setPosition(egostate.getPosition());
egoPosition.setRotation(egostate.getRotation());
m_mapOfCurrentPositions[m_egoStateNodeDescriptor] = egoPosition;
if ( (m_numberOfReceivedEgoStates % 30) == 0 ) {
NodeDescriptor nd("EgoCar (Trace)");
TransformGroup *tg = m_mapOfTraceablePositions[nd];
if (tg != NULL) {
Point3 color(0, 0, 1);
hesperia::threeD::models::Point *p = new hesperia::threeD::models::Point(NodeDescriptor("Trace"), egostate.getPosition(), color, 5);
tg->addChild(p);
}
}
}
}
if (c.getDataType() == Container::CONTOUREDOBJECTS) {
if (m_contouredObjectsNode != NULL) {
Lock l(m_rootMutex);
ContouredObjects cos = c.getData<ContouredObjects>();
vector<ContouredObject> listOfContouredObjects = cos.getContouredObjects();
vector<ContouredObject>::iterator it = listOfContouredObjects.begin();
m_contouredObjectsNode->deleteAllChildren();
while (it != listOfContouredObjects.end()) {
vector<Point3> contour = (*it).getContour();
vector<Point3>::iterator jt = contour.begin();
while (jt != contour.end()) {
m_contouredObjectsNode->addChild(new hesperia::threeD::models::Point(NodeDescriptor("Point"), (*jt), Point3(1, 0, 0), 2));
jt++;
}
it++;
}
}
}
if (c.getDataType() == Container::ROUTE) {
if (m_plannedRoute != NULL) {
Lock l(m_rootMutex);
Route r = c.getData<Route>();
vector<Point3> listOfVertices = r.getListOfPoints();
const uint32_t SIZE = listOfVertices.size();
if (SIZE > 0) {
m_plannedRoute->deleteAllChildren();
for (uint32_t i = 0; i < SIZE - 1; i++) {
Point3 posA = listOfVertices.at(i);
posA.setZ(0.05);
Point3 posB = listOfVertices.at(i+1);
posB.setZ(0.05);
m_plannedRoute->addChild(new hesperia::threeD::models::Line(NodeDescriptor(), posA, posB, Point3(0, 1, 0), 6));
}
}
}
}
if (c.getDataType() == Container::DRAW_LINE) {
if (m_lines != NULL) {
Lock l(m_rootMutex);
hesperia::data::environment::Line line = c.getData<Line>();
Point3 posA = line.getA();
posA.setZ(0.05);
Point3 posB = line.getB();
posB.setZ(0.05);
m_lines->addChild(new hesperia::threeD::models::Line(NodeDescriptor(), posA, posB, Point3(1, 0, 0), 6));
}
}
if (c.getDataType() == Container::OBSTACLE) {
if (m_obstaclesRoot != NULL) {
Lock l(m_rootMutex);
Obstacle obstacle = c.getData<Obstacle>();
switch (obstacle.getState()) {
case Obstacle::REMOVE:
{
// Remove obstacle.
map<uint32_t, Node*>::iterator result = m_mapOfObstacles.find(obstacle.getID());
if (result != m_mapOfObstacles.end()) {
// Remove child from scene graph node.
m_obstaclesRoot->removeChild(result->second);
// Remove entry from map.
m_mapOfObstacles.erase(result);
}
}
break;
case Obstacle::UPDATE:
{
map<uint32_t, Node*>::iterator result = m_mapOfObstacles.find(obstacle.getID());
if (result != m_mapOfObstacles.end()) {
// Remove child from scene graph node.
m_obstaclesRoot->removeChild(result->second);
// Remove entry from map.
m_mapOfObstacles.erase(result);
}
// Update obstacle.
TransformGroup *contourTG = new TransformGroup();
vector<Point3> contour = obstacle.getPolygon().getVertices();
// Close polygons.
Point3 p = contour.at(0);
contour.push_back(p);
for (uint32_t k = 0; k < contour.size() - 1; k++) {
Point3 A = contour.at(k); A.setZ(0.5);
Point3 B = contour.at(k+1); B.setZ(0.5);
contourTG->addChild(new hesperia::threeD::models::Line(NodeDescriptor(), A, B, Point3(0, 1, 0), 2));
}
m_mapOfObstacles[obstacle.getID()] = contourTG;
m_obstaclesRoot->addChild(contourTG);
}
break;
}
}
}
}
