TEST(SharedMemoryTest, ShareSpawn) {
const char* env_meta_path = ::getenv("test_meta_path");
if (env_meta_path) {
// child process!
{
std::string meta_path(env_meta_path);
std::cout << "I'm a child process(" << ::getpid()
<< "). meta_path=" << meta_path << std::endl;
SharedMemory memory;
memory.attach(meta_path);
EXPECT_FALSE(memory.is_null());
EXPECT_EQ(1ULL << 21, memory.get_size());
EXPECT_EQ(meta_path, memory.get_meta_path());
EXPECT_NE(0, memory.get_shmid());
EXPECT_NE(0, memory.get_shmkey());
EXPECT_EQ(0, memory.get_numa_node());
EXPECT_EQ(42, memory.get_block()[3]);
EXPECT_FALSE(memory.is_owned());
memory.get_block()[5] = 67;
}
::_exit(0); // for the same reason, this should terminate now.
return;
} else {
std::cout << "I'm a master process(" << ::getpid() << ")" << std::endl;
}
SharedMemory memory;
EXPECT_TRUE(memory.get_block() == nullptr);
EXPECT_TRUE(memory.is_null());
std::string meta_path = base_path() + std::string("_share_spawn");
COERCE_ERROR(memory.alloc(meta_path, 1ULL << 21, 0));
memory.get_block()[3] = 42;
posix_spawn_file_actions_t file_actions;
posix_spawnattr_t attr;
::posix_spawn_file_actions_init(&file_actions);
::posix_spawnattr_init(&attr);
std::string path = assorted::get_current_executable_path();
// execute this test
char* const new_argv[] = {
const_cast<char*>(path.c_str()),
const_cast<char*>("--gtest_filter=SharedMemoryTest.ShareSpawn"),
nullptr};
// with meta_path in environment variable
std::string meta("test_meta_path=");
meta += meta_path;
char* const new_envp[] = {
const_cast<char*>(meta.c_str()),
nullptr};
pid_t child_pid;
std::cout << "spawning: " << path << std::endl;
int ret = ::posix_spawn(&child_pid, path.c_str(), &file_actions, &attr, new_argv, new_envp);
EXPECT_EQ(0, ret);
if (ret == 0) {
EXPECT_NE(0, child_pid);
int status = 0;
pid_t result = ::waitpid(child_pid, &status, 0);
std::cout << "child process(" << child_pid << ") died. result=" << result << std::endl;
EXPECT_EQ(child_pid, result);
EXPECT_EQ(0, status);
}
assorted::memory_fence_acquire();
EXPECT_EQ(67, memory.get_block()[5]);
memory.release_block();
}
bool RecognitionModel::loadFromPath(const std::string &path) {
boost::filesystem::path dir(path);
if(!boost::filesystem::is_directory(dir)) {
ROS_ERROR("Not a directory: %s", boost_path_to_cstr(dir));
return false;
}
boost::filesystem::path meta_path(dir / "meta.xml");
if (!boost::filesystem::exists(meta_path)) {
ROS_ERROR("No meta.xml file found in %s", boost_path_to_cstr(dir));
return false;
}
int face_count = 0;
std::string model_type = "";
bool loaded = loadMetaFile(boost_path_to_cstr(meta_path), model_name, model_type, face_count);
if (!loaded || model_name.empty() || (face_count == 0) || (model_type != "kinect_pcl")) {
ROS_ERROR("invalid meta.xml in %s", boost_path_to_cstr(dir));
return false;
}
for(boost::filesystem::directory_iterator it(dir); it!=boost::filesystem::directory_iterator(); ++it)
{
boost::filesystem::path pref = *it;
const static std::string FACE_PREFIX = "dense_face_";
#if BOOST_VERSION >= 104600
if ((pref.filename().string().substr(0, FACE_PREFIX.length()) == FACE_PREFIX) && (pref.extension() == ".pcd")) {
#else
if ((pref.filename().substr(0, FACE_PREFIX.length()) == FACE_PREFIX) && (pref.extension() == ".pcd")) {
#endif
aspects.push_back(new ObjectAspect());
ObjectAspect* aspect = aspects.back();
aspect->fromFile(pref.string());
if (aspects.size() > static_cast<size_t>(face_count)) {
ROS_INFO("found more faces than specified in meta.xml. please check %s", boost_path_to_cstr(meta_path));
break;
}
}
}
if (aspects.size() < static_cast<size_t>(face_count)) {
ROS_ERROR("meta.xml specified %i faces, but only %i were found.", face_count, aspects.size());
return false;
}
if (aspects.empty()) {
ROS_ERROR("no model files in path %s", path.c_str());
return false;
}
return true;
}
bool RecognitionModel::matchAspects(ObjectAspect &other, Eigen::Matrix4f &model2scene) {
double minerror = DBL_MAX;
for (size_t i = 0; i < aspects.size(); i ++) {
ObjectAspect* model_aspect = aspects[i];
Eigen::Matrix4f trafo;
double error = model_aspect->matchAspects(other,trafo);
if (error < minerror) {
model2scene = trafo;
other.match = model_aspect;
minerror = error;
ROS_INFO("HIT %d, minerror: %f",(int)i, minerror);
}
}
return minerror != DBL_MAX;
}
