void RobotLink::createEntityForGeometryElement(const urdf::LinkConstPtr& link, const urdf::Geometry& geom, const urdf::Pose& origin, node*& entity)
{
offset_node = new node();
gpu_voxels::Vector3f scale;
KDL::Vector offset_pos = KDL::Vector(origin.position.x, origin.position.y, origin.position.z);
KDL::Rotation offset_rot = KDL::Rotation::Quaternion(origin.rotation.x, origin.rotation.y, origin.rotation.z, origin.rotation.w);
KDL::Frame offset_pose = KDL::Frame(offset_rot, offset_pos);
switch (geom.type)
{
case urdf::Geometry::SPHERE:
{
LOGGING_WARNING_C(RobotLog, RobotLink,
"Link " << std::string(link->name) <<
" has SPHERE geometry, which is not supported!" << endl);
break;
}
case urdf::Geometry::BOX:
{
LOGGING_WARNING_C(RobotLog, RobotLink,
"Link " << link->name <<
" has BOX geometry, which is not supported!" << endl);
break;
}
case urdf::Geometry::CYLINDER:
{
LOGGING_WARNING_C(RobotLog, RobotLink,
"Link " << link->name <<
" has CYLINDER geometry, which is not supported!" << endl);
break;
}
case urdf::Geometry::MESH:
{
const urdf::Mesh& mesh = static_cast<const urdf::Mesh&>(geom);
if ( mesh.filename.empty() )
return;
scale = gpu_voxels::Vector3f(mesh.scale.x, mesh.scale.y, mesh.scale.z);
fs::path p(mesh.filename);
fs::path pc_file = fs::path(p.stem().string() + std::string(".binvox"));
std::vector<Vector3f> link_cloud;
LOGGING_DEBUG_C(RobotLog, RobotLink, "Loading pointcloud of link " << pc_file.string() << endl);
if(!file_handling::PointcloudFileHandler::Instance()->loadPointCloud(
pc_file.string(), use_model_path_, link_cloud, false, Vector3f(), 1.0))
{
LOGGING_ERROR_C(RobotLog, RobotLink,
"Could not read file [" << pc_file.string() <<
"]. Adding single point instead..." << endl);
link_cloud.push_back(Vector3f());
}
link_pointclouds_.addCloud(link_cloud, false, name_);
entity->setHasData(true);
break;
}
default:
LOGGING_WARNING_C(RobotLog, RobotLink,
"Link " << link->name <<
" has unsupported geometry type " << geom.type << "." << endl);
break;
}
if ( entity )
{
offset_node->setScale(scale);
offset_node->setPose(offset_pose);
}
}
RobotLink::RobotLink(Robot* robot,
const urdf::LinkConstPtr& link,
const std::string& parent_joint_name,
bool visual,
bool collision,
bool use_model_path,
MetaPointCloud& link_pointclouds)
: robot_( robot )
, name_( link->name )
, parent_joint_name_( parent_joint_name )
, visual_node_( NULL )
, collision_node_( NULL )
, link_pointclouds_(link_pointclouds)
, pose_calculated_(false)
, use_model_path_(use_model_path)
{
pose_property_ = KDL::Frame();
visual_node_ = new node;
collision_node_ = new node;
// we prefer collisions over visuals
if (collision) createCollision(link);
else if (visual) createVisual(link);
// create description and fill in child_joint_names_ vector
std::stringstream desc;
if (parent_joint_name_.empty())
{
desc << "Root Link " << name_;
}
else
{
desc << "Link " << name_;
desc << " with parent joint " << parent_joint_name_;
}
if (link->child_joints.empty())
{
desc << " has no children.";
}
else
{
desc
<< " has "
<< link->child_joints.size();
if (link->child_joints.size() > 1)
{
desc << " child joints: ";
}
else
{
desc << " child joint: ";
}
std::vector<boost::shared_ptr<urdf::Joint> >::const_iterator child_it = link->child_joints.begin();
std::vector<boost::shared_ptr<urdf::Joint> >::const_iterator child_end = link->child_joints.end();
for ( ; child_it != child_end ; ++child_it )
{
urdf::Joint *child_joint = child_it->get();
if (child_joint && !child_joint->name.empty())
{
child_joint_names_.push_back(child_joint->name);
desc << child_joint->name << ((child_it+1 == child_end) ? "." : ", ");
}
}
}
if (hasGeometry())
{
if (visual_meshes_.empty())
{
desc << " This link has collision geometry but no visible geometry.";
}
else if (collision_meshes_.empty())
{
desc << " This link has visible geometry but no collision geometry.";
}
}
else
{
desc << " This link has NO geometry.";
}
LOGGING_DEBUG_C(RobotLog, RobotLink, desc << endl);
}
/*!
* \brief loadPointCloud loads a PCD file and returns the points in a vector.
* \param path Filename
* \param points points are written into this vector
* \param shift_to_zero If true, the pointcloud is shifted, so its minimum coordinates lie at zero
* \param offset_XYZ Additional transformation offset
* \return true if succeeded, false otherwise
*/
bool PointcloudFileHandler::loadPointCloud(const std::string _path, const bool use_model_path, std::vector<Vector3f> &points, const bool shift_to_zero,
const Vector3f &offset_XYZ, const float scaling)
{
std::string path;
// if param is true, prepend the environment variable GPU_VOXELS_MODEL_PATH
path = (getGpuVoxelsPath(use_model_path) / boost::filesystem::path(_path)).string();
LOGGING_DEBUG_C(
Gpu_voxels_helpers,
GpuVoxelsMap,
"Loading Pointcloud file " << path << " ..." << endl);
// is the file a simple xyz file?
std::size_t found = path.find(std::string("xyz"));
if (found!=std::string::npos)
{
if (!xyz_reader->readPointCloud(path, points))
{
return false;
}
}else{
// is the file a simple pcl pcd file?
std::size_t found = path.find(std::string("pcd"));
if (found!=std::string::npos)
{
#ifdef _BUILD_GVL_WITH_PCL_SUPPORT_
if (!pcd_reader->readPointCloud(path, points))
{
return false;
}
#else
LOGGING_ERROR_C(
Gpu_voxels_helpers,
GpuVoxelsMap,
"Your GPU-Voxels was built without PCD support!" << endl);
return false;
#endif
}else{
// is the file a binvox file?
std::size_t found = path.find(std::string("binvox"));
if (found!=std::string::npos)
{
if (!binvox_reader->readPointCloud(path, points))
{
return false;
}
}else{
LOGGING_ERROR_C(
Gpu_voxels_helpers,
GpuVoxelsMap,
path << " has no known file format." << endl);
return false;
}
}
}
if (shift_to_zero)
{
shiftPointCloudToZero(points);
}
for (size_t i = 0; i < points.size(); i++)
{
points[i].x = (scaling * points[i].x) + offset_XYZ.x;
points[i].y = (scaling * points[i].y) + offset_XYZ.y;
points[i].z = (scaling * points[i].z) + offset_XYZ.z;
}
return true;
}
