35 int main (
int argc,
char **argv)
37 ROS_INFO(
"Started laser_to_pc_nodelet");
38 ros::init(argc,argv,
"laser_to_pc_nodelet");
44 string pointcloud_output;
47 double min_x,min_y,max_x,max_y;
53 ros::NodeHandle private_node_handle_(
"~");
54 private_node_handle_.param(
"laser_input", laser_input,
string(
"laser_input"));
55 private_node_handle_.param(
"pointcloud_output", pointcloud_output,
string(
"pointcloud_output"));
56 private_node_handle_.param(
"frame_id", frame_id,
string(
"frame_id"));
57 private_node_handle_.param(
"min_x", min_x,
double(min_x));
58 private_node_handle_.param(
"min_y", min_y,
double(min_y));
59 private_node_handle_.param(
"max_x", max_x,
double(max_x));
60 private_node_handle_.param(
"max_y", max_y,
double(max_y));
61 private_node_handle_.param(
"min_z", las_to_pointcloud->
min_z,
double(las_to_pointcloud->
min_z));
62 private_node_handle_.param(
"max_z", las_to_pointcloud->
max_z,
double(las_to_pointcloud->
max_z));
65 ros::Subscriber sub_cloud = n.subscribe(laser_input.c_str(), 1000, &
laser_to_pc::filter, las_to_pointcloud);
68 ros::Publisher pub_message = n.advertise<sensor_msgs::PointCloud2>(pointcloud_output.c_str(), 10);
70 las_to_pointcloud->
pub_ptr=&pub_message;
73 tf::TransformListener listener;
75 las_to_pointcloud->
frame_id=frame_id;
78 PointXYZ point; point.z=0.0;
79 point.x=min_x; point.y=max_y;
80 las_to_pointcloud->
convex_hull.points.push_back(point);
81 point.x=max_x; point.y=max_y;
82 las_to_pointcloud->
convex_hull.points.push_back(point);
83 point.x=max_x; point.y=min_y;
84 las_to_pointcloud->
convex_hull.points.push_back(point);
85 point.x=min_x; point.y=min_y;
86 las_to_pointcloud->
convex_hull.points.push_back(point);
87 las_to_pointcloud->
convex_hull.header.frame_id=frame_id;
tf::TransformListener * listener_ptr
transform_listener pointer
PointCloud< PointXYZ > convex_hull
convex_hull for extraction
laser_to_pc class declaration
double min_z
z limits for extraction
ros::Publisher * pub_ptr
publisher pointer
int main(int argc, char **argv)
void filter(const sensor_msgs::LaserScanPtr &msg)
callback function