27 #ifndef _points_from_volume_nodelet_CPP_
28 #define _points_from_volume_nodelet_CPP_
55 void cloud_cb (
const sensor_msgs::PointCloud2ConstPtr & pcmsg_in)
57 pcl::PointCloud<pcl::PointXYZ> pc_in;
58 pcl::PCLPointCloud2 pcl_pc;
59 pcl_conversions::toPCL(*pcmsg_in, pcl_pc);
60 pcl::fromPCLPointCloud2(pcl_pc, pc_in);
62 pcl::PointCloud<pcl::PointXYZ> pc_filtered;
64 sensor_msgs::PointCloud2 pcmsg_out;
79 printf(
"-------Flag is now True-------\n");
93 int main(
int argc,
char **argv)
95 ros::init(argc, argv,
"points_from_volume_nodelet");
96 ros::NodeHandle n(
"~");
104 ROS_WARN_ONCE(
"Positive_offset not defined. Setting to default value '2.0'\n");
105 n.param(
"positive_offset",
positive, 2.0);
109 ROS_WARN_ONCE(
"Negative_offset not defined. Setting to default value '-0.1'\n");
110 n.param(
"negative_offset",
negative, -0.1);
114 ROS_WARN_ONCE(
"Flag_in_out not defined. Setting to default value 'false'\n");
115 n.param(
"flag_in_out",
flag,
false);
120 ros::Subscriber sub2 = n.subscribe (
"/point_cloud_input", 1,
cloud_cb);
124 ROS_WARN(
"No point cloud convexhull received yet...");
128 cloud_pub = n.advertise<sensor_msgs::PointCloud2>(
"/pcmsg_out", 1);
130 ros::Rate loop_rate(30);
void cb_points_convexhull(const sensor_msgs::PointCloud2ConstPtr &pcmsg_convexhull_in)
ConvexHull callback.
t_func_output convexhull_function(pcl::PointCloud< T > &pc_in, double positive_offset, double negative_offset, bool flag_in_out)
Extract points from the pcl that are inside the convexhull.
points_from_volume< pcl::PointXYZ > piv
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &pcmsg_in)
Point Cloud callback.
int main(int argc, char **argv)
Main code of the nodelet.
t_func_output set_convex_hull(pcl::PointCloud< T > &pc_in)
Set the convexhull.
pcl::PointCloud< T > get_pc_in_volume()
Points in the desired volume.
points_from_volume class declaration and auxiliary types