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 DataRenderer::draw(const Obstacle &o) {
Lock l(m_dataRendererMutex);
if (m_renderer != NULL) {
// Ignore other states.
if (o.getState() == Obstacle::UPDATE) {
// Update obstacle.
m_renderer->beginPainting();
m_renderer->setColor(Point3(0, 1, 0));
m_renderer->setLineWidth(2.0);
// Add additional height to lift the obstacle from the ground.
vector<Point3> points;
vector<Point3> originalPoints = o.getPolygon().getVertices();
vector<Point3>::iterator it = originalPoints.begin();
while (it != originalPoints.end()) {
Point3 p = (*it++);
p.setZ(0.5);
points.push_back(p);
}
m_renderer->drawPolyLine(points);
m_renderer->endPainting();
}
}
}
core::SharedPointer<core::wrapper::Image> OpenGLGrabber::getNextImage() {
if ( (m_sharedMemory.isValid()) && (m_sharedMemory->isValid()) ) {
m_sharedMemory->lock();
// Render the image right before grabbing it.
switch (m_render) {
case OpenGLGrabber::IN_CAR:
renderNextImageInCar();
m_FIFO_Obstacles.clear();
break;
case OpenGLGrabber::CHASE_CAR:
renderNextImageChaseCar();
m_FIFO_Obstacles.clear();
break;
case OpenGLGrabber::CHASE_CAR_SENSORS:
{
const uint32_t size = m_FIFO_Obstacles.getSize();
for(uint32_t i = 0; i < size; i++) {
Container c = m_FIFO_Obstacles.leave();
if (c.getDataType() == Container::OBSTACLE) {
Obstacle obstacle = c.getData<Obstacle>();
// Check if sensor FOV-"Obstacle":
if (obstacle.getID() >= 9000) {
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_sensors->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_sensors->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_sensors->addChild(contourTG);
}
break;
}
}
}
}
renderNextImageChaseCarSensors();
}
break;
}
// TODO Read pixels using BGRA!!!
glReadBuffer(GL_BACK);
glPixelStorei(GL_PACK_ALIGNMENT, 1);
glReadPixels(0, 0, m_image->getWidth(), m_image->getHeight(), GL_BGR, GL_UNSIGNED_BYTE, m_sharedMemory->getSharedMemory());
// Flip the image horizontally.
m_image->flipHorizontally();
m_sharedMemory->unlock();
}
return m_image;
}
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;
}
}
}
}