moveit2
The MoveIt Motion Planning Framework for ROS 2.
|
Computing a mask for a pointcloud that states which points are inside the robot. More...
#include <shape_mask.h>
Classes | |
struct | SeeShape |
struct | SortBodies |
Public Types | |
enum | { INSIDE = 0 , OUTSIDE = 1 , CLIP = 2 } |
The possible values of a mask computed for a point. More... | |
typedef std::function< bool(ShapeHandle, Eigen::Isometry3d &)> | TransformCallback |
Public Member Functions | |
ShapeMask (const TransformCallback &transform_callback=TransformCallback()) | |
Construct the filter. | |
virtual | ~ShapeMask () |
Destructor to clean up. | |
ShapeHandle | addShape (const shapes::ShapeConstPtr &shape, double scale=1.0, double padding=0.0) |
void | removeShape (ShapeHandle handle) |
void | setTransformCallback (const TransformCallback &transform_callback) |
void | maskContainment (const sensor_msgs::msg::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, the point is inside the robot. The point is outside if the mask element is OUTSIDE. | |
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 is in the frame corresponding to the TransformCallback. | |
int | getMaskContainment (const Eigen::Vector3d &pt) const |
Get the containment mask (INSIDE or OUTSIDE) value for an individual point. It is assumed the point is in the frame corresponding to the TransformCallback. | |
Protected Attributes | |
TransformCallback | transform_callback_ |
std::mutex | shapes_lock_ |
Protects, bodies_ and bspheres_. All public methods acquire this mutex for their whole duration. | |
std::set< SeeShape, SortBodies > | bodies_ |
std::vector< bodies::BoundingSphere > | bspheres_ |
Computing a mask for a pointcloud that states which points are inside the robot.
Definition at line 52 of file shape_mask.h.
typedef std::function<bool(ShapeHandle, Eigen::Isometry3d&)> point_containment_filter::ShapeMask::TransformCallback |
Definition at line 63 of file shape_mask.h.
anonymous enum |
The possible values of a mask computed for a point.
Enumerator | |
---|---|
INSIDE | |
OUTSIDE | |
CLIP |
Definition at line 56 of file shape_mask.h.
point_containment_filter::ShapeMask::ShapeMask | ( | const TransformCallback & | transform_callback = TransformCallback() | ) |
Construct the filter.
Definition at line 52 of file shape_mask.cpp.
|
virtual |
Destructor to clean up.
Definition at line 57 of file shape_mask.cpp.
point_containment_filter::ShapeHandle point_containment_filter::ShapeMask::addShape | ( | const shapes::ShapeConstPtr & | shape, |
double | scale = 1.0 , |
||
double | padding = 0.0 |
||
) |
Definition at line 75 of file shape_mask.cpp.
int point_containment_filter::ShapeMask::getMaskContainment | ( | const Eigen::Vector3d & | pt | ) | const |
Get the containment mask (INSIDE or OUTSIDE) value for an individual point. It is assumed the point is in the frame corresponding to the TransformCallback.
Definition at line 204 of file shape_mask.cpp.
int point_containment_filter::ShapeMask::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 is in the frame corresponding to the TransformCallback.
Definition at line 217 of file shape_mask.cpp.
void point_containment_filter::ShapeMask::maskContainment | ( | const sensor_msgs::msg::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, the point is inside the robot. The point is outside if the mask element is OUTSIDE.
Definition at line 129 of file shape_mask.cpp.
void point_containment_filter::ShapeMask::removeShape | ( | ShapeHandle | handle | ) |
Definition at line 114 of file shape_mask.cpp.
void point_containment_filter::ShapeMask::setTransformCallback | ( | const TransformCallback & | transform_callback | ) |
Definition at line 69 of file shape_mask.cpp.
|
protected |
Definition at line 120 of file shape_mask.h.
|
protected |
Definition at line 121 of file shape_mask.h.
|
mutableprotected |
Protects, bodies_ and bspheres_. All public methods acquire this mutex for their whole duration.
Definition at line 119 of file shape_mask.h.
|
protected |
Definition at line 116 of file shape_mask.h.