|
Protected Types inherited from robot_body_filter::FilterBase< sensor_msgs::LaserScan > |
using | ToStringFn = std::string(*)(const T &) |
| Type of function that converts anything to a string. More...
|
|
Protected Member Functions inherited from robot_body_filter::RobotBodyFilter< sensor_msgs::LaserScan > |
void | addRobotMaskFromUrdf (const std::string &urdfModel) |
| Update robot_shape_mask with the given URDF model. More...
|
|
void | clearRobotMask () |
| Remove all parts of the robot mask and clear internal shape and TF buffers. More...
|
|
void | computeAndPublishBoundingBox (const sensor_msgs::PointCloud2 &projectedPointCloud) const |
| Computation of the bounding box, debug boxes, and publishing of pointcloud without bounding box. More...
|
|
void | computeAndPublishBoundingSphere (const sensor_msgs::PointCloud2 &projectedPointCloud) const |
| Computation of the bounding sphere, debug spheres, and publishing of pointcloud without bounding sphere. More...
|
|
void | computeAndPublishLocalBoundingBox (const sensor_msgs::PointCloud2 &projectedPointCloud) const |
| Computation of the local bounding box, debug boxes, and publishing of pointcloud without bounding box. More...
|
|
void | computeAndPublishOrientedBoundingBox (const sensor_msgs::PointCloud2 &projectedPointCloud) const |
| Computation of the oriented bounding box, debug boxes, and publishing of pointcloud without bounding box. More...
|
|
bool | computeMask (const sensor_msgs::PointCloud2 &projectedPointCloud, std::vector< RayCastingShapeMask::MaskValue > &mask, const std::string &sensorFrame="") |
| Perform the actual computation of mask. More...
|
|
void | createBodyVisualizationMsg (const std::map< point_containment_filter::ShapeHandle, const bodies::Body * > &bodies, const ros::Time &stamp, const std_msgs::ColorRGBA &color, visualization_msgs::MarkerArray &markerArray) const |
|
ScaleAndPadding | getLinkInflationForBoundingBox (const std::string &linkName) const |
|
ScaleAndPadding | getLinkInflationForBoundingBox (const std::vector< std::string > &linkNames) const |
|
ScaleAndPadding | getLinkInflationForBoundingSphere (const std::string &linkName) const |
|
ScaleAndPadding | getLinkInflationForBoundingSphere (const std::vector< std::string > &linkNames) const |
|
ScaleAndPadding | getLinkInflationForContainsTest (const std::string &linkName) const |
|
ScaleAndPadding | getLinkInflationForContainsTest (const std::vector< std::string > &linkNames) const |
|
ScaleAndPadding | getLinkInflationForShadowTest (const std::string &linkName) const |
|
ScaleAndPadding | getLinkInflationForShadowTest (const std::vector< std::string > &linkNames) const |
|
bool | getShapeTransform (point_containment_filter::ShapeHandle shapeHandle, Eigen::Isometry3d &transform) const |
| Return the latest cached transform for the link corresponding to the given shape handle. More...
|
|
void | publishDebugMarkers (const ros::Time &scanTime) const |
|
void | publishDebugPointClouds (const sensor_msgs::PointCloud2 &projectedPointCloud, const std::vector< RayCastingShapeMask::MaskValue > &pointMask) const |
|
void | robotDescriptionUpdated (dynamic_reconfigure::ConfigConstPtr newConfig) |
| Callback handling update of the robot_description parameter using dynamic reconfigure. More...
|
|
bool | triggerModelReload (std_srvs::TriggerRequest &, std_srvs::TriggerResponse &) |
| Callback for ~reload_model service. Reloads the URDF from parameter. More...
|
|
void | updateTransformCache (const ros::Time &time, const ros::Time &afterScanTime=ros::Time(0)) |
| Update the cache of link transforms relative to filtering frame. More...
|
|
Protected Member Functions inherited from robot_body_filter::FilterBase< sensor_msgs::LaserScan > |
std::string | getParamVerbose (const std::string &name, const char *defaultValue, const std::string &unit="", bool *defaultUsed=nullptr, ToStringFn< std::string > valueToStringFn=&to_string) const |
| Get the value of the given filter parameter, falling back to the specified default value, and print out a ROS info/warning message with the loaded values. More...
|
|
ros::Duration | getParamVerbose (const std::string &name, const ros::Duration &defaultValue, const std::string &unit="", bool *defaultUsed=nullptr, ToStringFn< double > valueToStringFn=&to_string) const |
| Get the value of the given filter parameter, falling back to the specified default value, and print out a ROS info/warning message with the loaded values. More...
|
|
T | getParamVerbose (const std::string &name, const T &defaultValue=T(), const std::string &unit="", bool *defaultUsed=nullptr, ToStringFn< T > valueToStringFn=&to_string) const |
| Get the value of the given filter parameter, falling back to the specified default value, and print out a ROS info/warning message with the loaded values. More...
|
|
uint64_t | getParamVerbose (const std::string &name, const uint64_t &defaultValue, const std::string &unit="", bool *defaultUsed=nullptr, ToStringFn< int > valueToStringFn=&to_string) const |
| Get the value of the given filter parameter, falling back to the specified default value, and print out a ROS info/warning message with the loaded values. More...
|
|
unsigned int | getParamVerbose (const std::string &name, const unsigned int &defaultValue, const std::string &unit="", bool *defaultUsed=nullptr, ToStringFn< int > valueToStringFn=&to_string) const |
| Get the value of the given filter parameter, falling back to the specified default value, and print out a ROS info/warning message with the loaded values. More...
|
|
MapType | getParamVerboseMap (const std::string &name, const std::map< std::string, T > &defaultValue=std::map< std::string, T >(), const std::string &unit="", bool *defaultUsed=nullptr, ToStringFn< MapType > valueToStringFn=&to_string) const |
|
std::set< T > | getParamVerboseSet (const std::string &name, const std::set< T > &defaultValue=std::set< T >(), const std::string &unit="", bool *defaultUsed=nullptr, ToStringFn< std::vector< T >> valueToStringFn=&to_string) const |
| Get the value of the given filter parameter as a set of strings, falling back to the specified default value, and print out a ROS info/warning message with the loaded values. More...
|
|
bool | getParam (const std::string &name, bool &value) const |
|
bool | getParam (const std::string &name, double &value) const |
|
bool | getParam (const std::string &name, int &value) const |
|
bool | getParam (const std::string &name, std::string &value) const |
|
bool | getParam (const std::string &name, std::vector< double > &value) const |
|
bool | getParam (const std::string &name, std::vector< std::string > &value) const |
|
bool | getParam (const std::string &name, unsigned int &value) const |
|
bool | getParam (const std::string &name, XmlRpc::XmlRpcValue &value) const |
|
bool | loadConfiguration (XmlRpc::XmlRpcValue &config) |
|
Protected Attributes inherited from robot_body_filter::RobotBodyFilterLaserScan |
const std::unordered_map< std::string, CloudChannelType > | channelsToTransform { {"vp_", CloudChannelType::POINT} } |
|
laser_geometry::LaserProjection | laserProjector |
|
Protected Attributes inherited from robot_body_filter::RobotBodyFilter< sensor_msgs::LaserScan > |
ros::Publisher | boundingBoxDebugMarkerPublisher |
| Publisher of the debug bounding box markers. More...
|
|
ros::Publisher | boundingBoxMarkerPublisher |
| Publisher of the bounding box marker. More...
|
|
ros::Publisher | boundingBoxPublisher |
| Publisher of robot bounding box (relative to fixed frame). More...
|
|
ros::Publisher | boundingSphereDebugMarkerPublisher |
| Publisher of the debug bounding sphere markers. More...
|
|
ros::Publisher | boundingSphereMarkerPublisher |
| Publisher of the bounding sphere marker. More...
|
|
ros::Publisher | boundingSpherePublisher |
| Publisher of robot bounding sphere (relative to fixed frame). More...
|
|
double | cacheLookupBetweenScansRatio |
| If the scan is pointByPoint, set this variable to the ratio between scan start and end time you're looking for with getShapeTransform(). More...
|
|
bool | computeBoundingBox |
| Whether to compute bounding box of the robot. More...
|
|
bool | computeBoundingSphere |
| Whether to compute bounding sphere of the robot. More...
|
|
bool | computeDebugBoundingBox |
| Whether to compute debug bounding box of the robot. More...
|
|
bool | computeDebugBoundingSphere |
| Whether to compute debug bounding sphere of the robot. More...
|
|
bool | computeDebugLocalBoundingBox |
| Whether to compute debug local bounding box of the robot. More...
|
|
bool | computeDebugOrientedBoundingBox |
| Whether to compute debug oriented bounding box of the robot. More...
|
|
bool | computeLocalBoundingBox |
| Whether to compute local bounding box of the robot. More...
|
|
bool | computeOrientedBoundingBox |
| Whether to compute oriented bounding box of the robot. More...
|
|
ros::Publisher | debugBboxMarkerPublisher |
|
ros::Publisher | debugBsphereMarkerPublisher |
|
ros::Publisher | debugContainsMarkerPublisher |
|
ros::Publisher | debugPointCloudClipPublisher |
|
ros::Publisher | debugPointCloudInsidePublisher |
|
ros::Publisher | debugPointCloudShadowPublisher |
|
ros::Publisher | debugShadowMarkerPublisher |
|
ScaleAndPadding | defaultBboxInflation |
|
ScaleAndPadding | defaultBsphereInflation |
|
ScaleAndPadding | defaultContainsInflation |
|
ScaleAndPadding | defaultShadowInflation |
|
bool | failWithoutRobotDescription |
|
std::string | filteringFrame |
| Frame in which the filter is applied. For point-by-point scans, it has to be a fixed frame, otherwise, it can be the sensor frame. More...
|
|
std::string | fixedFrame |
| Fixed frame wrt the sensor frame. Usually base_link for stationary robots (or sensor frame if both robot and sensor are stationary). For mobile robots, it can be e.g. odom or map. More...
|
|
bool | keepCloudsOrganized |
|
std::set< std::string > | linksIgnoredEverywhere |
|
std::set< std::string > | linksIgnoredInBoundingBox |
|
std::set< std::string > | linksIgnoredInBoundingSphere |
|
std::set< std::string > | linksIgnoredInContainsTest |
|
std::set< std::string > | linksIgnoredInShadowTest |
|
ros::Publisher | localBoundingBoxDebugMarkerPublisher |
| Publisher of the debug local bounding box markers. More...
|
|
std::string | localBoundingBoxFrame |
| The frame in which local bounding box should be computed. More...
|
|
ros::Publisher | localBoundingBoxMarkerPublisher |
| Publisher of the local bounding box marker. More...
|
|
ros::Publisher | localBoundingBoxPublisher |
| Publisher of robot bounding box (relative to defined local frame). More...
|
|
double | maxDistance |
| The maximum distance of points from the sensor origin to apply this filter on (in meters). More...
|
|
double | minDistance |
| The minimum distance of points from the sensor to keep them (in meters). More...
|
|
std::shared_ptr< std::mutex > | modelMutex |
| A mutex that has to be locked in order to work with shapesToLinks or tfBuffer. More...
|
|
ros::Duration | modelPoseUpdateInterval |
| The interval between two consecutive model pose updates when processing a pointByPointScan. If set to zero, the model will be updated for each point separately (might be computationally exhaustive). If non-zero, it will only update the model once in this interval, which makes the masking algorithm a little bit less precise but more computationally affordable. More...
|
|
ros::NodeHandle | nodeHandle |
| Handle of the node this filter runs in. More...
|
|
std::set< std::string > | onlyLinks |
|
ros::Publisher | orientedBoundingBoxDebugMarkerPublisher |
| Publisher of the debug oriented bounding box markers. More...
|
|
ros::Publisher | orientedBoundingBoxMarkerPublisher |
| Publisher of the oriented bounding box marker. More...
|
|
ros::Publisher | orientedBoundingBoxPublisher |
| Publisher of robot bounding box (relative to fixed frame). More...
|
|
std::map< std::string, ScaleAndPadding > | perLinkBboxInflation |
|
std::map< std::string, ScaleAndPadding > | perLinkBsphereInflation |
|
std::map< std::string, ScaleAndPadding > | perLinkContainsInflation |
|
std::map< std::string, ScaleAndPadding > | perLinkShadowInflation |
|
bool | pointByPointScan |
| If true, suppose that every point in the scan was captured at a different time instant. Otherwise, the scan is assumed to be taken at once. More...
|
|
ros::NodeHandle | privateNodeHandle |
|
bool | publishBoundingBoxMarker |
| Whether to publish the bounding box marker. More...
|
|
bool | publishBoundingSphereMarker |
| Whether to publish the bounding sphere marker. More...
|
|
bool | publishDebugBboxMarker |
|
bool | publishDebugBsphereMarker |
|
bool | publishDebugContainsMarker |
|
bool | publishDebugPclClip |
|
bool | publishDebugPclInside |
|
bool | publishDebugPclShadow |
|
bool | publishDebugShadowMarker |
|
bool | publishLocalBoundingBoxMarker |
| Whether to publish the bounding box marker. More...
|
|
bool | publishNoBoundingBoxPointcloud |
| Whether to publish scan_point_cloud with robot bounding box cut out. More...
|
|
bool | publishNoBoundingSpherePointcloud |
| Whether to publish scan_point_cloud with robot bounding sphere cut out. More...
|
|
bool | publishNoLocalBoundingBoxPointcloud |
| Whether to publish scan_point_cloud with robot local bounding box cut out. More...
|
|
bool | publishNoOrientedBoundingBoxPointcloud |
| Whether to publish scan_point_cloud with robot oriented bounding box cut out. More...
|
|
bool | publishOrientedBoundingBoxMarker |
| Whether to publish the bounding box marker. More...
|
|
ros::Duration | reachableTransformTimeout |
| Timeout for reachable transforms. More...
|
|
ros::ServiceServer | reloadRobotModelServiceServer |
| Service server for reloading robot model. More...
|
|
bool | requireAllFramesReachable |
| Whether to process data when there are some unreachable frames. More...
|
|
std::string | robotDescriptionParam |
| Name of the parameter where the robot model can be found. More...
|
|
std::string | robotDescriptionUpdatesFieldName |
| Name of the field in the dynamic reconfigure message that contains robot model. More...
|
|
ros::Subscriber | robotDescriptionUpdatesListener |
| Subscriber for robot_description updates. More...
|
|
ros::Publisher | scanPointCloudNoBoundingBoxPublisher |
| Publisher of scan_point_cloud with robot bounding box cut out. More...
|
|
ros::Publisher | scanPointCloudNoBoundingSpherePublisher |
| Publisher of scan_point_cloud with robot bounding sphere cut out. More...
|
|
ros::Publisher | scanPointCloudNoLocalBoundingBoxPublisher |
| Publisher of scan_point_cloud with robot local bounding box cut out. More...
|
|
ros::Publisher | scanPointCloudNoOrientedBoundingBoxPublisher |
| Publisher of scan_point_cloud with robot oriented bounding box cut out. More...
|
|
std::string | sensorFrame |
| Frame of the sensor. For LaserScan version, it is automatically read from the incoming data. For PointCloud2, you have to specify it explicitly because the pointcloud could have already been transformed e.g. to the fixed frame. More...
|
|
std::unique_ptr< RayCastingShapeMask > | shapeMask |
| Tool for masking out 3D bodies out of point clouds. More...
|
|
std::set< point_containment_filter::ShapeHandle > | shapesIgnoredInBoundingBox |
|
std::set< point_containment_filter::ShapeHandle > | shapesIgnoredInBoundingSphere |
|
std::map< point_containment_filter::ShapeHandle, CollisionBodyWithLink > | shapesToLinks |
| A map that correlates shapes in robot_shape_mask to collision links in URDF. More...
|
|
std::shared_ptr< tf2_ros::Buffer > | tfBuffer |
| tf client More...
|
|
ros::Duration | tfBufferLength |
| tf buffer length More...
|
|
std::shared_ptr< TFFramesWatchdog > | tfFramesWatchdog |
| Watchdog for unreachable frames. More...
|
|
std::unique_ptr< tf2_ros::TransformListener > | tfListener |
| tf listener More...
|
|
ros::Time | timeConfigured |
| The time when the filter configuration has finished. More...
|
|
std::map< std::string, std::shared_ptr< Eigen::Isometry3d > > | transformCache |
| Caches any link->fixedFrame transforms after a scan message is received. Is queried by robot_shape_mask. Keys are CollisionBodyWithLink::cacheKey. More...
|
|
std::map< std::string, std::shared_ptr< Eigen::Isometry3d > > | transformCacheAfterScan |
| Caches any link->fixedFrame transforms at the time of scan end. Only used for pointByPoint scans. Is queried by robot_shape_mask. Keys are CollisionBodyWithLink::cacheKey. More...
|
|
ros::Duration | unreachableTransformTimeout |
| Timeout for unreachable transforms. More...
|
|
bool | configured_ |
|
std::string | filter_name_ |
|
std::string | filter_type_ |
|
string_map_t | params_ |
|