planning_models::KinematicModel::LinkModel* planning_models::KinematicModel::constructLinkModel(const urdf::Link *urdf_link)
{
ROS_ASSERT(urdf_link);
LinkModel *result = new LinkModel(this);
result->name_ = urdf_link->name;
if(urdf_link->collision && urdf_link->collision->geometry) {
result->collision_origin_transform_ = urdfPose2TFTransform(urdf_link->collision->origin);
result->shape_ = constructShape(urdf_link->collision->geometry.get());
} else if(urdf_link->visual && urdf_link->visual->geometry){
result->collision_origin_transform_ = urdfPose2TFTransform(urdf_link->visual->origin);
result->shape_ = constructShape(urdf_link->visual->geometry.get());
} else {
result->collision_origin_transform_.setIdentity();
result->shape_ = NULL;
}
if (urdf_link->parent_joint.get()) {
result->joint_origin_transform_ = urdfPose2TFTransform(urdf_link->parent_joint->parent_to_joint_origin_transform);
ROS_DEBUG_STREAM("Setting joint origin for " << result->name_ << " to "
<< result->joint_origin_transform_.getOrigin().x() << " "
<< result->joint_origin_transform_.getOrigin().y() << " "
<< result->joint_origin_transform_.getOrigin().z());
} else {
ROS_DEBUG_STREAM("Setting joint origin to identity for " << result->name_);
result->joint_origin_transform_.setIdentity();
}
return result;
}
bool robot_self_filter_color::SelfMask::configure(const std::vector<LinkInfo> &links)
{
// in case configure was called before, we free the memory
freeMemory();
sensor_pos_.setValue(0, 0, 0);
std::string content;
boost::shared_ptr<urdf::Model> urdfModel;
if (nh_.getParam("robot_description", content))
{
urdfModel = boost::shared_ptr<urdf::Model>(new urdf::Model());
if (!urdfModel->initString(content))
{
ROS_ERROR("Unable to parse URDF description!");
return false;
}
}
else
{
ROS_ERROR("Robot model not found! Did you remap 'robot_description'?");
return false;
}
std::stringstream missing;
// from the geometric model, find the shape of each link of interest
// and create a body from it, one that knows about poses and can
// check for point inclusion
for (unsigned int i = 0 ; i < links.size() ; ++i)
{
const urdf::Link *link = urdfModel->getLink(links[i].name).get();
if (!link)
{
missing << " " << links[i].name;
continue;
}
if (!(link->collision && link->collision->geometry))
{
ROS_WARN("No collision geometry specified for link '%s'", links[i].name.c_str());
continue;
}
shapes::Shape *shape = constructShape(link->collision->geometry.get());
if (!shape)
{
ROS_ERROR("Unable to construct collision shape for link '%s'", links[i].name.c_str());
continue;
}
SeeLink sl;
sl.body = bodies::createBodyFromShape(shape);
if (sl.body)
{
sl.name = links[i].name;
// collision models may have an offset, in addition to what TF gives
// so we keep it around
sl.constTransf = urdfPose2TFTransform(link->collision->origin);
sl.body->setScale(links[i].scale);
sl.body->setPadding(links[i].padding);
ROS_DEBUG_STREAM("Self see link name " << links[i].name << " padding " << links[i].padding);
sl.volume = sl.body->computeVolume();
sl.unscaledBody = bodies::createBodyFromShape(shape);
bodies_.push_back(sl);
}
else
ROS_WARN("Unable to create point inclusion body for link '%s'", links[i].name.c_str());
delete shape;
}
if (missing.str().size() > 0)
ROS_WARN("Some links were included for self mask but they do not exist in the model:%s", missing.str().c_str());
if (bodies_.empty())
ROS_WARN("No robot links will be checked for self mask");
// put larger volume bodies first -- higher chances of containing a point
std::sort(bodies_.begin(), bodies_.end(), SortBodies());
bspheres_.resize(bodies_.size());
bspheresRadius2_.resize(bodies_.size());
for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
ROS_DEBUG("Self mask includes link %s with volume %f", bodies_[i].name.c_str(), bodies_[i].volume);
//ROS_INFO("Self filter using %f padding and %f scaling", padd, scale);
return true;
}
