00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00033 #include <pointcloud_segmentation/filtering.h>
00034
00035 int main(int argc, char **argv)
00036 {
00037
00038 ros::init(argc, argv, "listener");
00039 ros::NodeHandle n;
00040
00041
00042 string pointcloud_input;
00043 string pointcloud_output;
00044 string laser_input;
00045 double min_x, max_x, min_y, max_y;
00046
00047
00048 filtering<PointXYZRGB> * filt = new filtering<PointXYZRGB> ();
00049
00050
00051 ros::NodeHandle private_node_handle_("~");
00052 private_node_handle_.param("pointcloud_input", pointcloud_input, string("pointcloud_input"));
00053 private_node_handle_.param("pointcloud_output", pointcloud_output, string("pointcloud_output"));
00054 private_node_handle_.param("frame_id", filt->frame_id, string("frame_id"));
00055
00056 private_node_handle_.param("voxel_size", filt->voxel_size, double(filt->voxel_size));
00057 private_node_handle_.param("min_x", min_x, double(min_x));
00058 private_node_handle_.param("max_x", max_x, double(max_x));
00059 private_node_handle_.param("min_y", min_y, double(min_y));
00060 private_node_handle_.param("max_y", max_y, double(max_y));
00061 private_node_handle_.param("min_z", filt->min_z, double(filt->min_z));
00062 private_node_handle_.param("max_z", filt->max_z, double(filt->max_z));
00063
00064
00065 PointXYZRGB point; point.z=0.0;
00066 point.x=min_x; point.y=max_y;
00067 filt->convex_hull.points.push_back(point);
00068 point.x=max_x; point.y=max_y;
00069 filt->convex_hull.points.push_back(point);
00070 point.x=max_x; point.y=min_y;
00071 filt->convex_hull.points.push_back(point);
00072 point.x=min_x; point.y=min_y;
00073 filt->convex_hull.points.push_back(point);
00074 filt->convex_hull.header.frame_id=filt->frame_id;
00075
00076
00077
00078 ros::Subscriber sub_cloud = n.subscribe(pointcloud_input.c_str(), 1000, &filtering<PointXYZRGB>::filter, filt);
00079
00080
00081 ros::Publisher pub_message = n.advertise<sensor_msgs::PointCloud2>(pointcloud_output.c_str(), 10);
00082
00083
00084 filt->pub_ptr=&pub_message;
00085
00086
00087 tf::TransformListener listener_center_bumper;
00088 filt->transform_listener_ptr=&listener_center_bumper;
00089
00090
00091 while (n.ok())
00092 {
00093 ros::spinOnce();
00094 }
00095
00096 return 0;
00097 }