/* Turn the model file into a model object. */
Model obtainModel(const string & path_to_model) {
// Control the file path
bfs::path model_path(path_to_model);
ModelParsers::testPath(model_path);
// Read the file and control its syntax
auto model_content = ModelParsers::readModel(model_path);
rng::sort(model_content);
Model result = parseModel(model_content);
result.name = model_path.stem().string();
return result;
}
MeshPersonVisual::MeshPersonVisual(const PersonVisualDefaultArgs& args) : PersonVisual(args), m_animationState(NULL), m_walkingSpeed(1.0), entity_(NULL)
{
m_childSceneNode = m_sceneNode->createChildSceneNode();
m_childSceneNode->setVisible(false);
std::string meshResource = "package://" ROS_PACKAGE_NAME "/media/animated_walking_man.mesh";
/// This is required to load referenced skeletons from package:// path
fs::path model_path(meshResource);
fs::path parent_path(model_path.parent_path());
Ogre::ResourceLoadingListener *newListener = new RosPackagePathResourceLoadingListener(parent_path),
*oldListener = Ogre::ResourceGroupManager::getSingleton().getLoadingListener();
Ogre::ResourceGroupManager::getSingleton().setLoadingListener(newListener);
bool loadFailed = rviz::loadMeshFromResource(meshResource).isNull();
Ogre::ResourceGroupManager::getSingleton().setLoadingListener(oldListener);
delete newListener;
// Create scene entity
static size_t count = 0;
std::stringstream ss;
ss << "mesh_person_visual" << count++;
std::string id = ss.str();
entity_ = m_sceneManager->createEntity(id, meshResource);
m_childSceneNode->attachObject(entity_);
// set up animation
setAnimationState("");
// set up material
ss << "Material";
Ogre::MaterialPtr default_material = Ogre::MaterialManager::getSingleton().create( ss.str(), "rviz" );
default_material->setReceiveShadows(false);
default_material->getTechnique(0)->setLightingEnabled(true);
default_material->getTechnique(0)->setAmbient( 0.5, 0.5, 0.5 );
materials_.insert( default_material );
entity_->setMaterial( default_material );
// set position
Ogre::Quaternion quat1; quat1.FromAngleAxis(Ogre::Degree(90), Ogre::Vector3(0,1,0));
Ogre::Quaternion quat2; quat2.FromAngleAxis(Ogre::Degree(-90), Ogre::Vector3(0,0,1));
m_childSceneNode->setOrientation(quat1 * quat2);
double scaleFactor = 0.243 * 1.75;
m_childSceneNode->setScale(Ogre::Vector3(scaleFactor, scaleFactor, scaleFactor));
m_childSceneNode->setPosition(Ogre::Vector3(0, 0, -1));
m_childSceneNode->setVisible(true);
}
bool Tracker::cb_track_object(pacman_vision_comm::track_object::Request& req, pacman_vision_comm::track_object::Response& res)
{
if (isDisabled()){
//Module was temporary disabled, notify the sad user, then exit
ROS_ERROR("[Tracker::%s]\tNode is globally disabled, this service is suspended!",__func__);
return false;
}
if (req.name.empty()){
ROS_ERROR("[Tracker::%s] You need to provide the name of the object you want to track!", __func__);
return false;
}
std::string models_path (ros::package::getPath("asus_scanner_models"));
if (!storage->searchObjName(req.name, index)){
ROS_ERROR("[Tracker::%s] Cannot find %s from the pool of already estimated objects, check spelling or run a Pose Estimation first!", __func__, req.name.c_str());
return false;
}
obj_name.first = (req.name);
std::vector<std::string> vst;
boost::split(vst, req.name, boost::is_any_of("_"), boost::token_compress_on);
obj_name.second = vst.at(0);
if (!storage->readObjTransformByIndex(index, transform)){
ROS_ERROR("[Tracker::%s] Cannot find %s transform from the pool of already estimated transforms, check spelling or run an estimation first!", __func__, req.name.c_str());
return false;
}
boost::filesystem::path model_path (models_path + "/" + obj_name.second + "/" + obj_name.second + ".pcd");
if (boost::filesystem::exists(model_path) && boost::filesystem::is_regular_file(model_path))
{
if (pcl::io::loadPCDFile(model_path.c_str(), *orig_model))
{
ROS_ERROR("[Tracker::%s] Error loading model %s",__func__, model_path.c_str());
return false;
}
}
else
{
ROS_ERROR("[Tracker::%s] Requested model (%s) does not exists in asus_scanner_models package",__func__, model_path.stem().c_str());
return false;
}
basic_config->get("downsampling_leaf_size", leaf);
computeModel();
//Get the minimum and maximum values on each of the 3 (x-y-z) dimensions of model
//also get model centroid
pcl::CentroidPoint<PX> mc;
std::vector<float> xvec,yvec,zvec;
for (size_t i=0; i<model->points.size(); ++i)
{
xvec.push_back(model->points[i].x);
yvec.push_back(model->points[i].y);
zvec.push_back(model->points[i].z);
mc.add(model->points[i]);
}
bounding_box->x1 = *std::min_element(xvec.begin(), xvec.end());
bounding_box->y1 = *std::min_element(yvec.begin(), yvec.end());
bounding_box->z1 = *std::min_element(zvec.begin(), zvec.end());
bounding_box->x2 = *std::max_element(xvec.begin(), xvec.end());
bounding_box->y2 = *std::max_element(yvec.begin(), yvec.end());
bounding_box->z2 = *std::max_element(zvec.begin(), zvec.end());
mc.get(model_centroid);
//init icps
icp.setUseReciprocalCorrespondences(false);
icp.setMaximumIterations(50);
icp.setTransformationEpsilon(1e-9);
icp.setEuclideanFitnessEpsilon(1e-9);
//Correspondence Rejector(s)
crd->setMaximumDistance(rej_distance);
icp.addCorrespondenceRejector(crd);
icp.setInputSource(model);
//Transformation Estimation
icp.setTransformationEstimation(teDQ);
storage->writeTrackedIndex(index);
//we are ready to start
started = true;
create_markers();
return true;
}
