63 const sensor_msgs::PointCloudConstPtr &msg)
const {
65 sensor_msgs::PointCloud2Ptr result(
new sensor_msgs::PointCloud2);
73 const sensor_msgs::LaserScanConstPtr &msg)
const {
77 sensor_msgs::PointCloud2Ptr result (
new sensor_msgs::PointCloud2);
83 sensor_msgs::LaserScan msg_changed(*msg);
85 for (
int i = 0; i < msg_changed.ranges.size(); i++) {
86 if (std::isnan(msg_changed.ranges[i])) {
87 msg_changed.ranges[i] =
101 const std::string base_frame,
const ros::Time time) {
108 ROS_INFO(
"%s: updated filter[%d] (%s)",
131 sensor_msgs::PointCloud2Ptr &result) {
139 ROS_WARN(
"%s: size of pointcloud after conversion to opencv is 0",
165 std::vector<bool> mask;
166 std::vector<int> count;
176 std::stringstream ss;
177 ss <<
"remaining points " << result->height <<
" (" <<
179 for (
int i = 0; i < count.size(); i++) {
180 ss <<
" - " << count[i];
191 const sensor_msgs::PointCloud2ConstPtr &msg,
192 const bool force_copy)
const {
195 int count = msg->height * msg->width;
196 int point_step = msg->point_step;
199 int types[4] = {-1, -1, -1, -1};
200 int offsets[4] = {0, 0, 0, 0};
201 for (
int i = 0; i < msg->fields.size(); i++) {
203 if ((msg->fields[i].name ==
"x") || (msg->fields[i].name ==
"X")) {
206 if ((msg->fields[i].name ==
"y") || (msg->fields[i].name ==
"Y")) {
209 if ((msg->fields[i].name ==
"z") || (msg->fields[i].name ==
"Z")) {
213 types[position] = msg->fields[i].datatype;
214 offsets[position] = msg->fields[i].offset ;
217 for (
int i = 0; i < 3; i++) {
224 bool simple_assign =
true;
225 for (
int i = 0; i < 3; i++) {
226 if ((types[0] != types[i]) || (offsets[i] != i * offsets[1])) {
227 simple_assign =
false;
232 if ((types[0] == sensor_msgs::PointField::FLOAT32) &&
234 cv::Mat result(count, 3, CV_32FC1,
235 const_cast<uint8_t*> (&(msg->data.front())), point_step);
238 ROS_INFO(
"%s: opencv - simple copy of pointcloud)",
242 result.copyTo(result_copy);
246 ROS_INFO(
"%s: opencv - reassignment of pointcloud)",
252 if ((types[0] == sensor_msgs::PointField::FLOAT64) &&
255 ROS_INFO(
"%s: opencv - reassignment of pointcloud" 259 cv::Mat (count, 3, CV_64FC1,
260 const_cast<uint8_t*> (&(msg->data.front())),
261 point_step).convertTo(result, CV_32FC1);
268 ROS_INFO(
"%s: opencv - complex copy of pointcloud",
271 cv::Mat result(count, 3, CV_32FC1);
273 for (
int i = 0; i < count; i++) {
275 for (
int j = 0; j < 3; j++) {
276 const void *ptr = &(msg->data[current_pos + offsets[j]]);
279 case sensor_msgs::PointField::FLOAT64:
280 value = float(*((
const double* ) ptr));
282 case sensor_msgs::PointField::FLOAT32:
283 value = *((
const float* ) ptr) ;
285 case sensor_msgs::PointField::INT32:
286 value = float(*((
const int32_t* ) ptr));
288 case sensor_msgs::PointField::UINT32:
289 value = float(*((
const uint32_t*) ptr));
291 case sensor_msgs::PointField::INT16:
292 value = float(*((
const int16_t* ) ptr));
294 case sensor_msgs::PointField::UINT16:
295 value = float(*((
const uint16_t*) ptr));
297 case sensor_msgs::PointField::INT8:
298 value = float(*((
const int8_t* ) ptr));
300 case sensor_msgs::PointField::UINT8:
301 value = float(*((
const uint8_t* ) ptr));
304 result.at<
double>(i,j) = value;
306 current_pos+= point_step;
314 const sensor_msgs::PointCloud2ConstPtr cloud,
315 const std::vector<bool> mask)
const {
317 int point_step = cloud->point_step;
319 int size_old = cloud->height * cloud->width;
320 if ((size_old == 0) && (point_step > 0)) {
321 size_old = cloud->data.size() / point_step;
324 int size_new = mask.size() <= size_old ? mask.size() : size_old;
326 for (
int i = 0; i < size_new; i++) {
327 if (mask[i]) {count++;}
332 sensor_msgs::PointCloud2Ptr result (
new sensor_msgs::PointCloud2);
333 result->header.frame_id = cloud->header.frame_id;
334 result->header.stamp = cloud->header.stamp;
335 result->is_bigendian = cloud->is_bigendian;
336 result->is_dense = cloud->is_dense;
337 result->height = size_new;
339 result->point_step = cloud->point_step;
340 result->row_step = cloud->point_step;
342 result->fields = cloud->fields;
343 result->data.resize(size_new * point_step);
345 uint8_t *pos_new = &result->data[0];
346 const uint8_t *pos_old_base = &cloud->data[0];
347 for (
int i = 0; i < size_old; i++) {
349 memcpy(pos_new, pos_old_base + i*point_step, point_step);
350 pos_new+= point_step;
void projectLaser(const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, double range_cutoff=-1.0, int channel_options=channel_option::Default)
std::vector< cPcdFilterPaFilter > filters_
internal filters for pointcloud
double laser_nan_replacement_value_
std::vector< int > pointcloudFilter(const cv::Mat &pointcloud, std::vector< bool > &mask) const
cv::Mat pcd_buffered_points_
ROSCPP_DECL const std::string & getName()
cPcdFilterPaRosParameter rosparams_
ros specific parameter
bool debugging_
flag for extended output
bool updateTf(const tf::TransformListener &tf_listener, const std::string base_frame, const ros::Time time=ros::Time::now())
sensor_msgs::PointCloud2Ptr convertCloud(const sensor_msgs::PointCloudConstPtr &msg) const
converts the given pointcloud to newer format
cPcdFilterPaRosFilters filters_
object for filter handling
const cv::Mat convertCloudToOpencv(const sensor_msgs::PointCloud2ConstPtr &msg, const bool force_copy=false) const
sensor_msgs::PointCloud2ConstPtr pcd_buffered_msg_
bool filterCloud(const sensor_msgs::PointCloud2ConstPtr &msg, sensor_msgs::PointCloud2Ptr &result)
filters the given pointcloud
static bool convertPointCloudToPointCloud2(const sensor_msgs::PointCloud &input, sensor_msgs::PointCloud2 &output)
bool update(const tf::TransformListener &tf_listener, const std::string base_frame, const ros::Time time, std::vector< cPcdFilterPaFilter > &result) const
~cPcdFilterPaRos()
default destructor
cPcdFilterPaRos()
default constructor
cPcdFilterPaParameter params_
specific parameter
sensor_msgs::PointCloud2Ptr applyMasktoCloud(const sensor_msgs::PointCloud2ConstPtr cloud, const std::vector< bool > mask) const