osg::ref_ptr<Resource::BulletShape> BulletNifLoader::load(const Nif::File& nif)
{
mShape = new Resource::BulletShape;
mCompoundShape.reset();
mStaticMesh.reset();
mAvoidStaticMesh.reset();
if (nif.numRoots() < 1)
{
warn("Found no root nodes in NIF.");
return mShape;
}
Nif::Record *r = nif.getRoot(0);
assert(r != nullptr);
Nif::Node *node = dynamic_cast<Nif::Node*>(r);
if (node == nullptr)
{
warn("First root in file was not a node, but a " +
r->recName + ". Skipping file.");
return mShape;
}
if (findBoundingBox(node))
{
std::unique_ptr<btCompoundShape> compound (new btCompoundShape);
std::unique_ptr<btBoxShape> boxShape(new btBoxShape(getbtVector(mShape->mCollisionBoxHalfExtents)));
btTransform transform = btTransform::getIdentity();
transform.setOrigin(getbtVector(mShape->mCollisionBoxTranslate));
compound->addChildShape(transform, boxShape.get());
boxShape.release();
mShape->mCollisionShape = compound.release();
return mShape;
}
else
{
bool autogenerated = hasAutoGeneratedCollision(node);
// files with the name convention xmodel.nif usually have keyframes stored in a separate file xmodel.kf (see Animation::addAnimSource).
// assume all nodes in the file will be animated
const auto filename = nif.getFilename();
const bool isAnimated = pathFileNameStartsWithX(filename);
handleNode(filename, node, 0, autogenerated, isAnimated, autogenerated);
if (mCompoundShape)
{
if (mStaticMesh)
{
btTransform trans;
trans.setIdentity();
std::unique_ptr<btCollisionShape> child(new Resource::TriangleMeshShape(mStaticMesh.get(), true));
mCompoundShape->addChildShape(trans, child.get());
child.release();
mStaticMesh.release();
}
mShape->mCollisionShape = mCompoundShape.release();
}
else if (mStaticMesh)
{
mShape->mCollisionShape = new Resource::TriangleMeshShape(mStaticMesh.get(), true);
mStaticMesh.release();
}
if (mAvoidStaticMesh)
{
mShape->mAvoidCollisionShape = new Resource::TriangleMeshShape(mAvoidStaticMesh.get(), false);
mAvoidStaticMesh.release();
}
return mShape;
}
}
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()));
}
