31 #include <sensor_msgs/PointCloud2.h> 33 #include <pcl/point_types.h> 34 #include <pcl/point_cloud.h> 36 #include <pcl/io/vtk_lib_io.h> 37 #include <pcl/filters/voxel_grid.h> 48 pcl::PointXYZ
operator-(
const pcl::PointXYZ& a,
const pcl::PointXYZ& b)
56 pcl::PointXYZ
operator+(
const pcl::PointXYZ& a,
const pcl::PointXYZ& b)
64 pcl::PointXYZ
operator*(
const pcl::PointXYZ& a,
const float& b)
73 std::vector<std::string>
split(
const std::string& input,
char delimiter)
75 std::istringstream stream(input);
78 std::vector<std::string> result;
79 while (std::getline(stream, field, delimiter))
81 result.push_back(field);
95 pub_cloud_ = neonavigation_common::compat::advertise<sensor_msgs::PointCloud2>(
97 pnh_,
"cloud", 1,
true);
99 pnh_.param(
"frame_id",
frame_id_, std::string(
"map"));
100 pnh_.param(
"objs",
file_, std::string(
""));
101 if (
file_.compare(
"") == 0)
107 pnh_.param(
"points_per_meter_sq",
ppmsq_, 600.0);
112 pnh_.param(
"scale",
scale_, 1.0);
135 sensor_msgs::PointCloud2
convertObj(
const std::vector<std::string>& files)
137 sensor_msgs::PointCloud2 pc_msg;
138 pcl::PolygonMesh::Ptr mesh(
new pcl::PolygonMesh());
139 pcl::PointCloud<pcl::PointXYZ>::Ptr pc(
new pcl::PointCloud<pcl::PointXYZ>());
140 pcl::PointCloud<pcl::PointXYZ>::Ptr pc_rs(
new pcl::PointCloud<pcl::PointXYZ>());
142 pcl::PointXYZ offset(static_cast<float>(offset_x_), static_cast<float>(offset_y_), static_cast<float>(offset_z_));
144 pc_rs->points.reserve(500000);
145 for (
auto& file : files)
147 auto ext = file.substr(file.find_last_of(
".") + 1);
157 for (
auto& p : pc->points)
163 pc_rs->points.push_back(p);
166 else if (ext ==
"obj")
168 if (pcl::io::loadPolygonFileOBJ(file, *mesh) == -1)
175 pcl::fromPCLPointCloud2(mesh->cloud, *pc);
176 pc_rs->header = pc->header;
177 for (
auto& p : pc->points)
184 std::uniform_real_distribution<float> ud(0.0, 1.0);
185 for (
auto& poly : mesh->polygons)
187 if (poly.vertices.size() != 3)
189 ROS_ERROR(
"Input mesh mush be triangle");
193 auto& p0 = pc->points[poly.vertices[0]];
194 auto& p1 = pc->points[poly.vertices[1]];
195 auto& p2 = pc->points[poly.vertices[2]];
200 float s = 0.5 * sqrtf(
201 powf(a.y * b.z - a.z * b.y, 2.0) +
202 powf(a.z * b.x - a.x * b.z, 2.0) +
203 powf(a.x * b.y - a.y * b.x, 2.0));
204 float numf = ppmsq_ * s;
206 if (numf - num > ud(engine_))
208 for (
int i = 0; i < num; i++)
210 float r0 = ud(engine_);
211 float r1 = ud(engine_);
217 pcl::PointXYZ p = p0 + (a * r0 + b * r1) + offset;
218 pc_rs->points.push_back(p);
223 pc_rs->width = pc_rs->points.size();
225 pc_rs->is_dense =
true;
228 pcl::PointCloud<pcl::PointXYZ>::Ptr pc_ds(
new pcl::PointCloud<pcl::PointXYZ>);
229 pcl::VoxelGrid<pcl::PointXYZ> ds;
230 ds.setInputCloud(pc_rs);
231 ds.setLeafSize(downsample_grid_, downsample_grid_, downsample_grid_);
237 ROS_INFO(
"pointcloud (%d points) has been generated from %d verticles",
244 int main(
int argc,
char** argv)
246 ros::init(argc, argv,
"obj_to_pointcloud");
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::Publisher pub_cloud_
ROSCPP_DECL void spin(Spinner &spinner)
int main(int argc, char **argv)
int loadPCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud)
std::random_device seed_gen_
std::default_random_engine engine_
std::vector< std::string > split(const std::string &input, char delimiter)
pcl::PointXYZ operator+(const pcl::PointXYZ &a, const pcl::PointXYZ &b)
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
ROSCPP_DECL void shutdown()
pcl::PointXYZ operator-(const pcl::PointXYZ &a, const pcl::PointXYZ &b)
sensor_msgs::PointCloud2 convertObj(const std::vector< std::string > &files)
pcl::PointXYZ operator*(const pcl::PointXYZ &a, const float &b)