35 #ifndef PR2_POINT_CLOUD_FOOTPRINT_FILTER_H 36 #define PR2_POINT_CLOUD_FOOTPRINT_FILTER_H 49 #include <pcl/point_types.h> 51 #include <sensor_msgs/PointCloud2.h> 67 ROS_ERROR(
"PR2PointCloudPointCloudFootprintFilter needs inscribed_radius to be set");
78 bool update(
const sensor_msgs::PointCloud2& input_scan2, sensor_msgs::PointCloud2& filtered_scan2)
80 if(&input_scan2 == &filtered_scan2){
81 ROS_ERROR(
"This filter does not currently support in place copying");
85 PointCloud input_scan, filtered_scan, laser_cloud;
89 std_msgs::Header header;
95 ROS_ERROR(
"Transform unavailable %s", ex.what());
99 filtered_scan.header = input_scan.header;
100 filtered_scan.points.resize (input_scan.points.size());
103 for (
unsigned int i=0; i < laser_cloud.points.size(); i++)
106 filtered_scan.points[num_pts] = input_scan.points[i];
112 filtered_scan.points.resize (num_pts);
133 #endif // PR2_POINT_CLOUD_FOOTPRINT_FILTER_H
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
bool getParam(const std::string &name, std::string &value)
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
pcl::PointCloud< pcl::PointXYZ > PointCloud
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)