void processTfTopic () {
map <string, vector<brics_3d::rsg::Attribute> >::iterator iter = objectClasses.begin();
for (iter = objectClasses.begin(); iter != objectClasses.end(); iter++) {
string objectFrameId = iter->first;
tf::StampedTransform transform;
try{
tfListener.lookupTransform(rootFrameId, objectFrameId, ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_WARN("%s",ex.what());
continue;
}
if ( (ros::Time::now() - transform.stamp_) > maxTFCacheDuration ) { //simply ignore outdated TF frames
ROS_WARN("TF found for %s. But it is outdated. Skipping it.", iter->first.c_str());
continue;
}
ROS_INFO("TF found for %s.", iter->first.c_str());
/* query */
vector<brics_3d::rsg::Attribute> queryAttributes;;
queryAttributes = iter->second;
vector<brics_3d::SceneObject> resultObjects;
myWM.getSceneObjects(queryAttributes, resultObjects);
// ROS_INFO("Number of boxes: %i " , static_cast<unsigned int>(resultObjects.size()));
/* associate */
unsigned int index = -1;
double minSquardDistanceToExistingObjects = std::numeric_limits<double>::max();
const double* matrixPtr;
for (unsigned int i = 0; i < static_cast<unsigned int>(resultObjects.size()) ; ++i) {
matrixPtr = resultObjects[i].transform->getRawData();
double squardDistanceToExistingObjects;
double xPercieved = transform.getOrigin().x();
double yPercieved = transform.getOrigin().y();
double zPercieved = transform.getOrigin().z();
double xStored = matrixPtr[12];
double yStored = matrixPtr[13];
double zStored = matrixPtr[14];
squardDistanceToExistingObjects = (xPercieved - xStored) * (xPercieved - xStored) +
(yPercieved - yStored) * (yPercieved - yStored) +
(zPercieved - zStored) * (zPercieved - zStored);
if (squardDistanceToExistingObjects < minSquardDistanceToExistingObjects) {
minSquardDistanceToExistingObjects = squardDistanceToExistingObjects;
index = i;
}
}
ROS_INFO("Shortest distance %lf to found result object %i.", minSquardDistanceToExistingObjects, index);
if (minSquardDistanceToExistingObjects < (associationDistanceTreshold * associationDistanceTreshold) ) {
/* update existing */
ROS_INFO("Updating existing scene object with object ID: %i", resultObjects[index].id);
brics_3d::IHomogeneousMatrix44::IHomogeneousMatrix44Ptr newTransform(new brics_3d::HomogeneousMatrix44());
tfTransformToHomogeniousMatrix(transform, newTransform);
myWM.insertTransform(resultObjects[index].id, newTransform);
} else {
/* insert */
ROS_INFO("Inserting new scene object");
brics_3d::rsg::Shape::ShapePtr boxShape(new brics_3d::rsg::Box(cubeSize, cubeSize, cubeSize)); // in [m]
brics_3d::rsg::Shape::ShapePtr targetAreaBoxShape(new brics_3d::rsg::Box(targetAreaSizeX, targetAreaSizeY, targetAreaSizeZ)); // in [m]
brics_3d::IHomogeneousMatrix44::IHomogeneousMatrix44Ptr initialTransform(new brics_3d::HomogeneousMatrix44());
tfTransformToHomogeniousMatrix(transform, initialTransform);
brics_3d::SceneObject tmpSceneObject;
if ( (iter->first.compare(startFrameId) == 0) || (iter->first.compare(auxiliaryFrameId) == 0) || (iter->first.compare(goalFrameId) == 0)) {
tmpSceneObject.shape = targetAreaBoxShape;
} else {
tmpSceneObject.shape = boxShape;
}
// tmpSceneObject.shape = boxShape;
tmpSceneObject.transform = initialTransform;
tmpSceneObject.parentId = myWM.getRootNodeId(); // hook in after root node
tmpSceneObject.attributes.clear();
tmpSceneObject.attributes = iter->second;
unsigned int returnedId;
myWM.addSceneObject(tmpSceneObject, returnedId);
}
}
/* query */
vector<brics_3d::rsg::Attribute> queryAttributes;
vector<brics_3d::SceneObject> resultObjects;
queryAttributes.push_back(brics_3d::rsg::Attribute("shapeType","Box"));
// queryAttributes.push_back(brics_3d::rsg::Attribute("color","red"));
myWM.getSceneObjects(queryAttributes, resultObjects);
ROS_INFO("Total number of boxes: %i " , static_cast<unsigned int>(resultObjects.size()));
}
bool Projectile::onAdd()
{
if(!Parent::onAdd())
return false;
if( !mDataBlock )
{
Con::errorf("Projectile::onAdd - Fail - Not datablock");
return false;
}
if (isServerObject())
{
ShapeBase* ptr;
if (Sim::findObject(mSourceObjectId, ptr))
{
mSourceObject = ptr;
// Since we later do processAfter( mSourceObject ) we must clearProcessAfter
// if it is deleted. SceneObject already handles this in onDeleteNotify so
// all we need to do is register for the notification.
deleteNotify( ptr );
}
else
{
if (mSourceObjectId != -1)
Con::errorf(ConsoleLogEntry::General, "Projectile::onAdd: mSourceObjectId is invalid");
mSourceObject = NULL;
}
// If we're on the server, we need to inherit some of our parent's velocity
//
mCurrTick = 0;
}
else
{
if (bool(mDataBlock->projectileShape))
{
mProjectileShape = new TSShapeInstance(mDataBlock->projectileShape, isClientObject());
if (mDataBlock->activateSeq != -1)
{
mActivateThread = mProjectileShape->addThread();
mProjectileShape->setTimeScale(mActivateThread, 1);
mProjectileShape->setSequence(mActivateThread, mDataBlock->activateSeq, 0);
}
}
if (mDataBlock->particleEmitter != NULL)
{
ParticleEmitter* pEmitter = new ParticleEmitter;
pEmitter->onNewDataBlock(mDataBlock->particleEmitter,false);
if (pEmitter->registerObject() == false)
{
Con::warnf(ConsoleLogEntry::General, "Could not register particle emitter for particle of class: %s", mDataBlock->getName());
delete pEmitter;
pEmitter = NULL;
}
mParticleEmitter = pEmitter;
}
if (mDataBlock->particleWaterEmitter != NULL)
{
ParticleEmitter* pEmitter = new ParticleEmitter;
pEmitter->onNewDataBlock(mDataBlock->particleWaterEmitter,false);
if (pEmitter->registerObject() == false)
{
Con::warnf(ConsoleLogEntry::General, "Could not register particle emitter for particle of class: %s", mDataBlock->getName());
delete pEmitter;
pEmitter = NULL;
}
mParticleWaterEmitter = pEmitter;
}
}
if (mSourceObject.isValid())
processAfter(mSourceObject);
// Setup our bounding box
if (bool(mDataBlock->projectileShape) == true)
mObjBox = mDataBlock->projectileShape->bounds;
else
mObjBox = Box3F(Point3F(0, 0, 0), Point3F(0, 0, 0));
MatrixF initialTransform( true );
initialTransform.setPosition( mCurrPosition );
setTransform( initialTransform ); // calls resetWorldBox
addToScene();
if ( PHYSICSMGR )
mPhysicsWorld = PHYSICSMGR->getWorld( isServerObject() ? "server" : "client" );
return true;
}
void RenderNode::issueOperations(OpenGLRenderer& renderer, T& handler) {
if (mDisplayList->isEmpty()) {
DISPLAY_LIST_LOGD("%*sEmpty display list (%p, %s)", handler.level() * 2, "",
this, getName());
return;
}
#if HWUI_NEW_OPS
const bool drawLayer = false;
#else
const bool drawLayer = (mLayer && (&renderer != mLayer->renderer.get()));
#endif
// If we are updating the contents of mLayer, we don't want to apply any of
// the RenderNode's properties to this issueOperations pass. Those will all
// be applied when the layer is drawn, aka when this is true.
const bool useViewProperties = (!mLayer || drawLayer);
if (useViewProperties) {
const Outline& outline = properties().getOutline();
if (properties().getAlpha() <= 0
|| (outline.getShouldClip() && outline.isEmpty())
|| properties().getScaleX() == 0
|| properties().getScaleY() == 0) {
DISPLAY_LIST_LOGD("%*sRejected display list (%p, %s)", handler.level() * 2, "",
this, getName());
return;
}
}
handler.startMark(getName());
#if DEBUG_DISPLAY_LIST
const Rect& clipRect = renderer.getLocalClipBounds();
DISPLAY_LIST_LOGD("%*sStart display list (%p, %s), localClipBounds: %.0f, %.0f, %.0f, %.0f",
handler.level() * 2, "", this, getName(),
clipRect.left, clipRect.top, clipRect.right, clipRect.bottom);
#endif
LinearAllocator& alloc = handler.allocator();
int restoreTo = renderer.getSaveCount();
handler(new (alloc) SaveOp(SaveFlags::MatrixClip),
PROPERTY_SAVECOUNT, properties().getClipToBounds());
DISPLAY_LIST_LOGD("%*sSave %d %d", (handler.level() + 1) * 2, "",
SaveFlags::MatrixClip, restoreTo);
if (useViewProperties) {
setViewProperties<T>(renderer, handler);
}
#if HWUI_NEW_OPS
LOG_ALWAYS_FATAL("legacy op traversal not supported");
#else
bool quickRejected = properties().getClipToBounds()
&& renderer.quickRejectConservative(0, 0, properties().getWidth(), properties().getHeight());
if (!quickRejected) {
Matrix4 initialTransform(*(renderer.currentTransform()));
renderer.setBaseTransform(initialTransform);
if (drawLayer) {
handler(new (alloc) DrawLayerOp(mLayer),
renderer.getSaveCount() - 1, properties().getClipToBounds());
} else {
const int saveCountOffset = renderer.getSaveCount() - 1;
const int projectionReceiveIndex = mDisplayList->projectionReceiveIndex;
for (size_t chunkIndex = 0; chunkIndex < mDisplayList->getChunks().size(); chunkIndex++) {
const DisplayList::Chunk& chunk = mDisplayList->getChunks()[chunkIndex];
std::vector<ZDrawRenderNodeOpPair> zTranslatedNodes;
buildZSortedChildList(chunk, zTranslatedNodes);
issueOperationsOf3dChildren(ChildrenSelectMode::NegativeZChildren,
initialTransform, zTranslatedNodes, renderer, handler);
for (size_t opIndex = chunk.beginOpIndex; opIndex < chunk.endOpIndex; opIndex++) {
DisplayListOp *op = mDisplayList->getOps()[opIndex];
#if DEBUG_DISPLAY_LIST
op->output(handler.level() + 1);
#endif
handler(op, saveCountOffset, properties().getClipToBounds());
if (CC_UNLIKELY(!mProjectedNodes.empty() && projectionReceiveIndex >= 0 &&
opIndex == static_cast<size_t>(projectionReceiveIndex))) {
issueOperationsOfProjectedChildren(renderer, handler);
}
}
issueOperationsOf3dChildren(ChildrenSelectMode::PositiveZChildren,
initialTransform, zTranslatedNodes, renderer, handler);
}
}
}
#endif
DISPLAY_LIST_LOGD("%*sRestoreToCount %d", (handler.level() + 1) * 2, "", restoreTo);
handler(new (alloc) RestoreToCountOp(restoreTo),
PROPERTY_SAVECOUNT, properties().getClipToBounds());
DISPLAY_LIST_LOGD("%*sDone (%p, %s)", handler.level() * 2, "", this, getName());
handler.endMark();
}
