| 182 | } |
| 183 | |
| 184 | void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) { |
| 185 | static int msg_num = 0; |
| 186 | if (++msg_num % 10 != 0) return; |
| 187 | |
| 188 | pcl::PointCloud<pcl::PointXYZ> pts2, pts3; |
| 189 | pcl::fromROSMsg(*msg, pts2); |
| 190 | |
| 191 | // Filter unwanted points |
| 192 | for (int i = 0; i < pts2.points.size(); ++i) { |
| 193 | // if (pts2[i].z > 0.0 && pts2[i].z < 0.1 || pts2[i].z > 3.5 || pts2[i].y < -3.5) |
| 194 | // continue; |
| 195 | // if (pts2[i].z < 0.0) continue; |
| 196 | // if (pts2[i].z > 1.5 && pts2[i].x < 5.0) continue; |
| 197 | // if (pts2[i].y < 0 && pts2[i].y > -1 && pts2[i].z > 0.1 && pts2[i].x > 7.8) |
| 198 | // continue; |
| 199 | // if (pts2[i].y > 2.8) continue; |
| 200 | pts->push_back(pts2[i]); |
| 201 | } |
| 202 | |
| 203 | pcl::VoxelGrid<pcl::PointXYZ> sor; |
| 204 | sor.setInputCloud(pts); |
| 205 | sor.setLeafSize(0.1f, 0.1f, 0.1f); |
| 206 | sor.filter(pts3); |
| 207 | |
| 208 | pts->points = pts3.points; |
| 209 | pts->width = pts->points.size(); |
| 210 | pts->height = 1; |
| 211 | pts->is_dense = true; |
| 212 | pts->header.frame_id = "world"; |
| 213 | |
| 214 | sensor_msgs::PointCloud2 cloud; |
| 215 | pcl::toROSMsg(*pts, cloud); |
| 216 | cloud_pub.publish(cloud); |
| 217 | } |
| 218 | |
| 219 | void ewokCallback(const visualization_msgs::MarkerArrayConstPtr& msg) { |
| 220 | auto marker = msg->markers[0]; |