void MSF::poseCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& pose)
{
if(onPose) onPose(pose->pose.pose.orientation, pose->pose.pose.position);
geometry_msgs::PoseStamped dstPose;
/*dstPose.pose.orientation.x = pose->pose.pose.orientation.x;
dstPose.pose.orientation.y = pose->pose.pose.orientation.y;
dstPose.pose.orientation.z = pose->pose.pose.orientation.z;
dstPose.pose.orientation.w = sqrt(1.0 - 0.4 * 0.4);*/
/*calcOrientMSF(pose->pose.pose.orientation, dstPose.pose.orientation);
mavros_pose.publish(dstPose);
if(svo->stage == 3)
{
double z = pose->pose.pose.position.z;
/*if(!zref)
{
if(zct < 20)
zsum += z;
else
{
zref = zsum / (double)zct + 0.5;
zsum = 0.0;
ROS_INFO("Zref = %f", zref);
}
}*/
/*zpos = z;
if(!(zct & 7))
{
ROS_INFO("z = %f, th = %f", z, th);
}
zct++;
}
else
{
zpos = 0;
//zref = 0;
zsum = 0;
zct = 0;
} */
}
void App::onAfterLoadScene(const Any& any, const String& sceneName) {
Array<shared_ptr<Surface>> allSurfaces;
Array<shared_ptr<Surface2D>> ignore;
onPose(allSurfaces, ignore);
G3D::AABox aabox;
Surface::getBoxBounds(allSurfaces, aabox);
const float diameter = aabox.extent().max();
const Point3 center = aabox.center();
const float pad = 0.10f;
const Vector3 extent = Vector3::one() * diameter * (1.0f + pad);
aabox = AABox(center - extent / 2, center + extent / 2);
Box octtreeBounds(aabox);
m_svo->init(renderDevice, size_t(SVO_POOL_SIZE) * 1024 * 1024, SVO_MAX_DEPTH, size_t(SVO_FRAGBUFFER_SIZE) * 1024 * 1024);
m_svo->prepare(renderDevice, activeCamera(), octtreeBounds, 0.0f, /*-(float)previousSimTimeStep()*/ -0.016667f);
Surface::renderIntoSVO(renderDevice, allSurfaces, m_svo);
m_svo->complete(renderDevice, "SVO_downsampleValues.glc");
}
Any Scene::load(const std::string& scene) {
std::string filename;
clear();
m_modelTable.clear();
m_name = scene;
bool isFilename = endsWith(toLower(scene), ".scn.any") || endsWith(toLower(scene), ".Scene.Any");
if (isFilename) {
filename = scene;
} else {
const std::string* f = filenameTable().getPointer(scene);
if (f == NULL) {
throw "No scene with name '" + scene + "' found in (" +
stringJoin(filenameTable().getKeys(), ", ") + ")";
}
filename = *f;
}
Any any;
any.load(filename);
{
const std::string& n = any.get("name", filename);
// Ensure that this name appears in the filename table if it does not already,
// so that it can be loaded by name in the future.
if (! filenameTable().containsKey(n)) {
filenameTable().set(n, filename);
}
}
m_sourceAny = any;
// Load the lighting environment (do this before loading entities, since some of them may
// be lights that will enter this array)
bool hasEnvironmentMap = false;
if (any.containsKey("localLightingEnvironment")) {
m_localLightingEnvironment = any["localLightingEnvironment"];
hasEnvironmentMap = any["localLightingEnvironment"].containsKey("environmentMap");
}
// Load the models
if (any.containsKey("models")) {
Any models = any["models"];
if (models.size() > 0) {
for (Any::AnyTable::Iterator it = models.table().begin(); it.isValid(); ++it) {
const std::string name = it->key;
Any v = it->value;
createModel(v, name);
}
}
}
// Instance the models
if (any.containsKey("entities")) {
Any entities = any["entities"];
if (entities.size() > 0) {
for (Table<std::string, Any>::Iterator it = entities.table().begin(); it.isValid(); ++it) {
const std::string& name = it->key;
const std::string& entityType = it->value.name();
createEntity(entityType, name, it->value);
}
}
}
shared_ptr<Texture> skyboxTexture = Texture::whiteCube();
Any skyAny;
// Use the environment map as a skybox if there isn't one already, and vice versa
Array<shared_ptr<Skybox> > skyboxes;
getTypedEntityArray<Skybox>(skyboxes);
if ( skyboxes.size() == 0 ) {
if (any.containsKey("skybox")) {
createEntity("Skybox", "skybox", any["skybox"]);
m_skybox = typedEntity<Skybox>("skybox");
} else if (hasEnvironmentMap) {
m_skybox = Skybox::create("skybox", this, Array<ScaledTexture>(m_localLightingEnvironment.environmentMapArray[0]), Array<SimTime>(0.0), 0, SplineExtrapolationMode::CLAMP, false, false);
insert(m_skybox);
} else {
m_skybox = Skybox::create("skybox", this, Array<ScaledTexture>(ScaledTexture(Texture::whiteCube(), 1.0f)), Array<SimTime>(0.0), 0, SplineExtrapolationMode::CLAMP, false, false);
insert(m_skybox);
}
}
if (any.containsKey("environmentMap")) {
throw std::string("environmentMap field has been replaced with localLightingEnvironment");
}
// Default to using the skybox as an environment map if none is specified.
if (! hasEnvironmentMap) {
m_localLightingEnvironment.environmentMapArray.append(ScaledTexture(m_skybox->keyframeArray()[0].texture, m_skybox->keyframeArray()[0].constant));
}
//////////////////////////////////////////////////////
if (m_cameraArray.size() == 0) {
// Create a default camera, back it up from the origin
m_cameraArray.append(Camera::create("camera"));
m_cameraArray.last()->setFrame(CFrame::fromXYZYPRDegrees(0,1,-5,0,-5));
}
setTime(any.get("time", 0.0));
m_lastVisibleChangeTime = m_lastLightChangeTime = m_lastStructuralChangeTime = System::time();
m_defaultCameraName = (std::string)any.get("defaultCamera", "");
// Set the initial positions, repeating a few times to allow
// objects defined relative to others to reach a fixed point
for (int i = 0; i < 3; ++i) {
for (int e = 0; e < m_entityArray.size(); ++e) {
m_entityArray[e]->onSimulation(m_time, nan());
}
}
// Pose objects so that they have bounds.
{
Array< shared_ptr<Surface> > ignore;
onPose(ignore);
}
return any;
}
