43 : transform_callback_(transform_callback), next_handle_(1), min_handle_(1)
54 for (std::set<SeeShape>::const_iterator it =
bodies_.begin(); it !=
bodies_.end(); ++it)
66 double scale,
double padding)
77 std::pair<std::set<SeeShape, SortBodies>::iterator,
bool> insert_op =
bodies_.insert(ss);
78 if (!insert_op.second)
79 ROS_ERROR(
"Internal error in management of bodies in ShapeMask. This is a serious error.");
101 std::map<ShapeHandle, std::set<SeeShape, SortBodies>::iterator>::iterator it =
used_handles_.find(handle);
104 delete it->second->body;
110 ROS_ERROR(
"Unable to remove shape handle %u", handle);
114 const Eigen::Vector3d& sensor_origin,
115 const double min_sensor_dist,
const double max_sensor_dist,
116 std::vector<int>& mask)
119 const unsigned int np = data_in.data.size() / data_in.point_step;
123 std::fill(mask.begin(), mask.end(), (int)
OUTSIDE);
129 for (std::set<SeeShape>::const_iterator it =
bodies_.begin(); it !=
bodies_.end(); ++it)
135 <<
" without a body");
137 ROS_ERROR_STREAM_NAMED(
"shape_mask",
"Missing transform for shape " << it->body->getType() <<
" with handle " 142 it->body->setPose(tmp);
143 it->body->computeBoundingSphere(
bspheres_[j++]);
150 const double radiusSquared = bound.
radius * bound.
radius;
160 for (
int i = 0; i < (int)np; ++i)
162 Eigen::Vector3d pt = Eigen::Vector3d(*(iter_x + i), *(iter_y + i), *(iter_z + i));
163 double d = pt.norm();
165 if (d < min_sensor_dist || d > max_sensor_dist)
167 else if ((bound.
center - pt).squaredNorm() < radiusSquared)
168 for (std::set<SeeShape>::const_iterator it =
bodies_.begin(); it !=
bodies_.end() && out ==
OUTSIDE; ++it)
169 if (it->body->containsPoint(pt))
181 for (std::set<SeeShape>::const_iterator it =
bodies_.begin(); it !=
bodies_.end() && out ==
OUTSIDE; ++it)
182 if (it->body->containsPoint(pt))
~ShapeMask()
Destructor to clean up.
#define ROS_ERROR_STREAM_NAMED(name, args)
ShapeHandle addShape(const shapes::ShapeConstPtr &shape, double scale=1.0, double padding=0.0)
void setTransformCallback(const TransformCallback &transform_callback)
int getMaskContainment(double x, double y, double z) const
Get the containment mask (INSIDE or OUTSIDE) value for an individual point. It is assumed the point i...
void freeMemory()
Free memory.
void setScale(double scale)
boost::function< bool(ShapeHandle, Eigen::Affine3d &)> TransformCallback
boost::mutex shapes_lock_
void mergeBoundingSpheres(const std::vector< BoundingSphere > &spheres, BoundingSphere &mergedSphere)
std::vector< bodies::BoundingSphere > bspheres_
Body * createBodyFromShape(const shapes::Shape *shape)
std::map< ShapeHandle, std::set< SeeShape, SortBodies >::iterator > used_handles_
void maskContainment(const sensor_msgs::PointCloud2 &data_in, const Eigen::Vector3d &sensor_pos, const double min_sensor_dist, const double max_sensor_dist, std::vector< int > &mask)
Compute the containment mask (INSIDE or OUTSIDE) for a given pointcloud. If a mask element is INSIDE...
virtual double computeVolume() const =0
std::set< SeeShape, SortBodies > bodies_
void removeShape(ShapeHandle handle)
TransformCallback transform_callback_
void setPadding(double padd)
ShapeMask(const TransformCallback &transform_callback=TransformCallback())
Construct the filter.
std::shared_ptr< const Shape > ShapeConstPtr