30 #ifndef ROBOT_SELF_FILTER_SELF_MASK_ 31 #define ROBOT_SELF_FILTER_SELF_MASK_ 33 #include <pcl/point_types.h> 35 #include <sensor_msgs/PointCloud2.h> 38 #include <boost/bind.hpp> 39 #include <boost/filesystem.hpp> 67 tf::Vector3(pose.position.x, pose.position.y, pose.position.z));
78 result =
new shapes::Sphere(dynamic_cast<const urdf::Sphere*>(geom)->radius);
82 urdf::Vector3 dim =
dynamic_cast<const urdf::Box*
>(geom)->dim;
87 result =
new shapes::Cylinder(dynamic_cast<const urdf::Cylinder*>(geom)->radius,
88 dynamic_cast<const urdf::Cylinder*>(geom)->
length);
92 const urdf::Mesh *mesh =
dynamic_cast<const urdf::Mesh*
>(geom);
93 if (!mesh->filename.empty())
101 res = retriever.
get(mesh->filename);
112 ROS_WARN(
"Retrieved empty mesh for resource '%s'", mesh->filename.c_str());
115 boost::filesystem::path model_path(mesh->filename);
116 std::string ext = model_path.extension().string();
117 if (ext ==
".dae" || ext ==
".DAE") {
124 ROS_ERROR(
"Failed to load mesh '%s'", mesh->filename.c_str());
134 ROS_ERROR(
"Unknown geometry type: %d", (
int)geom->type);
144 template <
typename Po
intT>
153 body = unscaledBody = NULL;
192 mask.resize(data_in.points.size());
194 std::fill(mask.begin(), mask.end(), (int)
OUTSIDE);
199 maskAuxContainment(data_in, mask);
213 void maskIntersection(
const PointCloud& data_in,
const std::string &sensor_frame,
const double min_sensor_dist,
214 std::vector<int> &mask,
const boost::function<
void(
const tf::Vector3&)> &intersectionCallback = NULL)
216 mask.resize(data_in.points.size());
217 if (bodies_.empty()) {
218 std::fill(mask.begin(), mask.end(), (int)
OUTSIDE);
223 assumeFrame(header, sensor_frame, min_sensor_dist);
224 if (sensor_frame.empty())
225 maskAuxContainment(data_in, mask);
227 maskAuxIntersection(data_in, mask, intersectionCallback);
240 std::vector<int> &mask,
const boost::function<
void(
const tf::Vector3&)> &intersectionCallback = NULL)
242 mask.resize(data_in.points.size());
244 std::fill(mask.begin(), mask.end(), (int)
OUTSIDE);
248 assumeFrame(header, sensor_pos, min_sensor_dist);
249 maskAuxIntersection(data_in, mask, intersectionCallback);
258 const unsigned int bs = bodies_.size();
261 for (
unsigned int i = 0 ; i < bs ; ++i)
265 ROS_ERROR(
"WaitForTransform timed out from %s to %s after 100ms. Error string: %s", bodies_[i].
name.c_str(), header.frame_id.c_str(), err.c_str());
273 tf_.lookupTransform(header.frame_id, bodies_[i].name, header.stamp, transf);
278 ROS_ERROR(
"Unable to lookup transform from %s to %s. Exception: %s", bodies_[i].
name.c_str(), header.frame_id.c_str(), ex.what());
282 bodies_[i].body->setPose(transf * bodies_[i].constTransf);
283 bodies_[i].unscaledBody->setPose(transf * bodies_[i].constTransf);
286 computeBoundingSpheres();
296 sensor_pos_ = sensor_pos;
297 min_sensor_dist_ = min_sensor_dist;
302 void assumeFrame(
const std_msgs::Header& header,
const std::string &sensor_frame,
const double min_sensor_dist)
308 ROS_ERROR(
"WaitForTransform timed out from %s to %s after 100ms. Error string: %s", sensor_frame.c_str(), header.frame_id.c_str(), err.c_str());
309 sensor_pos_.setValue(0, 0, 0);
317 tf_.lookupTransform(header.frame_id, sensor_frame, header.stamp, transf);
323 ROS_ERROR(
"Unable to lookup transform from %s to %s. Exception: %s", sensor_frame.c_str(), header.frame_id.c_str(), ex.what());
326 min_sensor_dist_ = min_sensor_dist;
334 const unsigned int bs = bodies_.size();
336 for (
unsigned int j = 0 ; out ==
OUTSIDE && j < bs ; ++j)
337 if (bodies_[j].body->containsPoint(pt))
354 return getMaskIntersection(
tf::Vector3(x, y, z), intersectionCallback);
362 const unsigned int bs = bodies_.size();
367 for (
unsigned int j = 0 ; out ==
OUTSIDE && j < bs ; ++j)
368 if (bodies_[j].unscaledBody->containsPoint(pt))
377 if (lng < min_sensor_dist_)
383 std::vector<tf::Vector3> intersections;
384 for (
unsigned int j = 0 ; out ==
OUTSIDE && j < bs ; ++j)
385 if (bodies_[j].body->intersectsRay(pt, dir, &intersections, 1))
387 if (dir.
dot(sensor_pos_ - intersections[0]) >= 0.0)
389 if (intersectionCallback)
390 intersectionCallback(intersections[0]);
396 for (
unsigned int j = 0 ; out ==
OUTSIDE && j < bs ; ++j)
397 if (bodies_[j].body->containsPoint(pt))
408 for (
unsigned int i = 0 ; i < bodies_.size() ; ++i)
409 frames.push_back(bodies_[i].name);
417 for (
unsigned int i = 0 ; i < bodies_.size() ; ++i)
420 delete bodies_[i].body;
421 if (bodies_[i].unscaledBody)
422 delete bodies_[i].unscaledBody;
434 sensor_pos_.setValue(0, 0, 0);
439 if (nh_.getParam(
"robot_description", content))
442 if (!urdfModel->initString(content))
444 ROS_ERROR(
"Unable to parse URDF description!");
450 ROS_ERROR(
"Robot model not found! Did you remap 'robot_description'?");
454 std::stringstream missing;
459 for (
unsigned int i = 0 ; i < links.size() ; ++i)
461 const urdf::Link *link = urdfModel->getLink(links[i].
name).get();
464 missing <<
" " << links[i].name;
468 if (!(link->collision && link->collision->geometry))
470 ROS_WARN(
"No collision geometry specified for link '%s'", links[i].name.c_str());
478 ROS_ERROR(
"Unable to construct collision shape for link '%s'", links[i].name.c_str());
487 sl.name = links[i].name;
493 sl.body->setScale(links[i].
scale);
494 sl.body->setPadding(links[i].
padding);
495 ROS_INFO_STREAM(
"Self see link name " << links[i].name <<
" padding " << links[i].padding);
496 sl.volume = sl.body->computeVolume();
498 bodies_.push_back(sl);
501 ROS_WARN(
"Unable to create point inclusion body for link '%s'", links[i].name.c_str());
506 if (missing.str().size() > 0)
507 ROS_WARN(
"Some links were included for self mask but they do not exist in the model:%s", missing.str().c_str());
510 ROS_WARN(
"No robot links will be checked for self mask");
513 std::sort(bodies_.begin(), bodies_.end(), SortBodies());
515 bspheres_.resize(bodies_.size());
516 bspheresRadius2_.resize(bodies_.size());
518 for (
unsigned int i = 0 ; i < bodies_.size() ; ++i)
519 ROS_DEBUG(
"Self mask includes link %s with volume %f", bodies_[i].
name.c_str(), bodies_[i].volume);
530 const unsigned int bs = bodies_.size();
531 for (
unsigned int i = 0 ; i < bs ; ++i)
533 bodies_[i].body->computeBoundingSphere(bspheres_[i]);
534 bspheresRadius2_[i] = bspheres_[i].radius * bspheres_[i].radius;
542 const unsigned int bs = bodies_.size();
543 const unsigned int np = data_in.points.size();
552 for (
int i = 0 ; i < (int)np ; ++i)
557 for (
unsigned int j = 0 ; out ==
OUTSIDE && j < bs ; ++j)
558 if (bodies_[j].body->containsPoint(pt))
568 const unsigned int bs = bodies_.size();
569 const unsigned int np = data_in.points.size();
580 for (
int i = 0 ; i < (int)np ; ++i)
590 for (
unsigned int j = 0 ; out ==
OUTSIDE && j < bs ; ++j)
591 if (bodies_[j].unscaledBody->containsPoint(pt)) {
593 std::cout <<
"Point " << i <<
" in unscaled body part " << bodies_[j].name << std::endl;
603 if (lng < min_sensor_dist_) {
611 std::vector<tf::Vector3> intersections;
612 for (
unsigned int j = 0 ; out ==
OUTSIDE && j < bs ; ++j) {
613 if (bodies_[j].body->intersectsRay(pt, dir, &intersections, 1))
615 if (dir.
dot(sensor_pos_ - intersections[0]) >= 0.0)
618 callback(intersections[0]);
620 if(print) std::cout <<
"Point " << i <<
" shadowed by body part " << bodies_[j].name << std::endl;
626 for (
unsigned int j = 0 ; out ==
OUTSIDE && j < bs ; ++j)
627 if (bodies_[j].body->containsPoint(pt)) {
628 if(print) std::cout <<
"Point " << i <<
" in scaled body part " << bodies_[j].name << std::endl;
void getLinkNames(std::vector< std::string > &frames) const
Get the set of link names that have been instantiated for self filtering.
~SelfMask(void)
Destructor to clean up.
tf::Transform constTransf
Definition of a cylinder.
void assumeFrame(const std_msgs::Header &header, const std::string &sensor_frame, const double min_sensor_dist)
Assume subsequent calls to getMaskX() will be in the frame passed to this function. The frame in which the sensor is located is optional.
tf::TransformListener & tf_
void freeMemory(void)
Free memory.
void maskAuxContainment(const PointCloud &data_in, std::vector< int > &mask)
Perform the actual mask computation.
void maskIntersection(const PointCloud &data_in, const tf::Vector3 &sensor_pos, const double min_sensor_dist, std::vector< int > &mask, const boost::function< void(const tf::Vector3 &)> &intersectionCallback=NULL)
Compute the intersection mask for a given pointcloud. If a mask element can have one of the values IN...
bool operator()(const SeeLink &b1, const SeeLink &b2)
bodies::Body * unscaledBody
int getMaskIntersection(const tf::Vector3 &pt, const boost::function< void(const tf::Vector3 &)> &intersectionCallback=NULL) const
Get the intersection mask (INSIDE, OUTSIDE or SHADOW) value for an individual point. No setup is performed, assumeFrame() should be called before use.
ROSCONSOLE_DECL void print(FilterBase *filter, void *logger, Level level, const char *file, int line, const char *function, const char *fmt,...) ROSCONSOLE_PRINTF_ATTRIBUTE(7
TFSIMD_FORCE_INLINE void setValue(const tfScalar &x, const tfScalar &y, const tfScalar &z)
void maskIntersection(const PointCloud &data_in, const std::string &sensor_frame, const double min_sensor_dist, std::vector< int > &mask, const boost::function< void(const tf::Vector3 &)> &intersectionCallback=NULL)
Compute the intersection mask for a given pointcloud. If a mask element can have one of the values IN...
bool configure(const std::vector< LinkInfo > &links)
Configure the filter.
Definition of a sphere that bounds another object.
Mesh * createMeshFromBinaryStlData(const char *data, unsigned int size)
Load a mesh from a binary STL stream. Normals are recomputed and repeating vertices are identified...
SelfMask(tf::TransformListener &tf, const std::vector< LinkInfo > &links)
Construct the filter.
Mesh * createMeshFromBinaryDAE(const char *filename)
Load a mesh from a binary DAE file. Normals are recomputed and repeating vertices are identified...
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
int getMaskContainment(const tf::Vector3 &pt) const
Get the containment mask (INSIDE or OUTSIDE) value for an individual point. No setup is performed...
void mergeBoundingSpheres(const std::vector< BoundingSphere > &spheres, BoundingSphere &mergedSphere)
Compute a bounding sphere to enclose a set of bounding spheres.
A basic definition of a shape. Shapes are considered centered at origin.
TFSIMD_FORCE_INLINE tfScalar dot(const Vector3 &v) const
TFSIMD_FORCE_INLINE tfScalar distance2(const Vector3 &v) const
static shapes::Shape * constructShape(const urdf::Geometry *geom)
boost::shared_array< uint8_t > data
int getMaskContainment(double x, double y, double z) const
Get the containment mask (INSIDE or OUTSIDE) value for an individual point. No setup is performed...
Body * createBodyFromShape(const shapes::Shape *shape)
Create a body from a given shape.
static tf::Transform urdfPose2TFTransform(const urdf::Pose &pose)
pcl::PointCloud< PointT > PointCloud
void maskContainment(const PointCloud &data_in, std::vector< int > &mask)
Compute the containment mask (INSIDE or OUTSIDE) for a given pointcloud. If a mask element is INSIDE...
std::vector< bodies::BoundingSphere > bspheres_
std::vector< SeeLink > bodies_
MemoryResource get(const std::string &url)
Computing a mask for a pointcloud that states which points are inside the robot.
int getMaskIntersection(double x, double y, double z, const boost::function< void(const tf::Vector3 &)> &intersectionCallback=NULL) const
Get the intersection mask (INSIDE, OUTSIDE or SHADOW) value for an individual point. No setup is performed, assumeFrame() should be called before use.
#define ROS_INFO_STREAM(args)
std::vector< double > bspheresRadius2_
void assumeFrame(const std_msgs::Header &header, const tf::Vector3 &sensor_pos, const double min_sensor_dist)
Assume subsequent calls to getMaskX() will be in the frame passed to this function. Also specify which possition to assume for the sensor (frame is not needed)
void maskAuxIntersection(const PointCloud &data_in, std::vector< int > &mask, const boost::function< void(const tf::Vector3 &)> &callback)
Perform the actual mask computation.
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
void computeBoundingSpheres(void)
Compute bounding spheres for the checked robot links.
TFSIMD_FORCE_INLINE tfScalar length() const
void assumeFrame(const std_msgs::Header &header)
Assume subsequent calls to getMaskX() will be in the frame passed to this function. The frame in which the sensor is located is optional.