void robot_self_filter_color::SelfMask::assumeFrame(const std::string& frame_id, const ros::Time& stamp, const std::string &sensor_frame, double min_sensor_dist)
{
assumeFrame(frame_id,stamp);
std::string err;
if(!tf_.waitForTransform(frame_id, sensor_frame, stamp, ros::Duration(.1), ros::Duration(.01), &err)) {
ROS_ERROR("WaitForTransform timed out from %s to %s after 100ms. Error string: %s", sensor_frame.c_str(), frame_id.c_str(), err.c_str());
sensor_pos_.setValue(0, 0, 0);
}
//transform should be there
// compute the origin of the sensor in the frame of the cloud
try
{
tf::StampedTransform transf;
tf_.lookupTransform(frame_id, sensor_frame, stamp, transf);
sensor_pos_ = transf.getOrigin();
}
catch(tf::TransformException& ex)
{
sensor_pos_.setValue(0, 0, 0);
ROS_ERROR("Unable to lookup transform from %s to %s. Exception: %s", sensor_frame.c_str(), frame_id.c_str(), ex.what());
}
min_sensor_dist_ = min_sensor_dist;
}
void robot_self_filter_color::SelfMask::maskContainment(const pcl::PointCloud<pcl::PointXYZRGB>& data_in, std::vector<int> &mask)
{
mask.resize(data_in.points.size());
if (bodies_.empty())
std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
else
{
assumeFrame(data_in.header.frame_id,data_in.header.stamp);
maskAuxContainment(data_in, mask);
}
}
void robot_self_filter_color::SelfMask::maskIntersection(const pcl::PointCloud<pcl::PointXYZRGB>& data_in, const tf::Vector3 &sensor_pos, const double min_sensor_dist,
std::vector<int> &mask, const boost::function<void(const tf::Vector3&)> &callback)
{
mask.resize(data_in.points.size());
if (bodies_.empty())
std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
else
{
assumeFrame(data_in.header.frame_id, data_in.header.stamp, sensor_pos, min_sensor_dist);
maskAuxIntersection(data_in, mask, callback);
}
}
void robot_self_filter::SelfMask::maskIntersection(const pcl::PointCloud<pcl::PointXYZ>& data_in, const std::string &sensor_frame, const double min_sensor_dist,
std::vector<int> &mask, const boost::function<void(const Eigen::Vector3d&)> &callback)
{
mask.resize(data_in.points.size());
if (bodies_.empty()) {
std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
}
else
{
assumeFrame(data_in.header.frame_id, pcl_conversions::fromPCL(data_in.header).stamp, sensor_frame, min_sensor_dist);
if (sensor_frame.empty())
maskAuxContainment(data_in, mask);
else
maskAuxIntersection(data_in, mask, callback);
}
}
void robot_self_filter_color::SelfMask::assumeFrame(const std::string& frame_id, const ros::Time& stamp, const tf::Vector3 &sensor_pos, double min_sensor_dist)
{
assumeFrame(frame_id,stamp);
sensor_pos_ = sensor_pos;
min_sensor_dist_ = min_sensor_dist;
}
