34 #define PFLN {printf("%s %d\n",__FILE__, __LINE__);}
83 sensor_msgs::PointCloud2 msg;
90 int main(
int argc,
char **argv)
92 ros::init(argc, argv,
"nodelet");
93 ros::NodeHandle n(
"~");
97 for (
int i=0; i<5; i++)
100 sprintf(str,
"/laserscan%d", i);
101 if (ros::names::remap(str)!=str)
108 for (
int i=0; i<5; i++)
111 sprintf(str,
"/pointcloud%d", i);
112 if (ros::names::remap(str)!=str)
117 std::vector<ros::Subscriber> sub;
119 for (
int i=0; i<5; i++)
124 sprintf(str,
"/laserscan%d", i);
126 ROS_INFO(
"Subscribing to %s", (ros::names::remap(str)).c_str());
132 for (
int i=0; i<5; i++)
137 sprintf(str,
"/pointcloud%d", i);
139 ROS_INFO(
"Subscribing to %s", (ros::names::remap(str)).c_str());
140 ROS_INFO(
"REcebeu pointcloud");
148 ROS_ERROR(
"No /laserscan[0 4] or /pointcloud[0 4] where remapped. Aborting...");
154 if (!n.hasParam(
"acc_frame"))
156 ROS_ERROR(
"Didn't introduce a accumulation frame");
166 if (!n.hasParam(
"distance_from"))
167 ROS_WARN(
"distance for accumulation 15 meters");
171 if (!n.hasParam(
"timer_value"))
172 ROS_WARN(
"Frequency of publication 2.0 sec");
182 if (!n.hasParam(
"voxel_size"))
183 ROS_WARN(
"Voxel_size Default 0.4 meter");
188 if (!n.hasParam(
"removed_from"))
190 ROS_ERROR(
"Remove from frame not Defined");
194 n.getParam(
"removed_from",removed_from);
204 tf::TransformListener listener(n,ros::Duration(10));
214 ros::Rate loop_rate(50);
std::string _acc_frame_id
nav_msgs::Odometry odometry_
bool process_point_cloud[5]
void pointcloud_Callback(const sensor_msgs::PointCloud2Ptr &image)
Callback from the PointCloud subscribed topic.
pcl::PointCloud< pcl::PointXYZRGB > pcl_pc_acc
void OdometryCallBack(const nav_msgs::OdometryConstPtr &odom_in)
Callback from the Odometry subscribed topic.
int remove_points_from_pointcloud(pcl::PointCloud< pcl::PointXYZRGB > pcl_pc, float dist, std::string frame_id)
Function the removes points from cloud This function calculated the origin of the given accumulation ...
tf::TransformListener * p_listener
int pointcloud_accumulated(sensor_msgs::LaserScan &scan_in, std::string acc_frame_id)
Function that receive a LaserScan and Accumulation Frame It converts the laserScan to a pcl::PointXYZ...
void laserscan_Callback(const sensor_msgs::LaserScanPtr &scan_in)
Callback from the LaserScan subscribed topic.
ros::Publisher cloud_pc_pointcloud
bool process_laser_scan[5]
int main(int argc, char **argv)
void Remove_Publish(const ros::TimerEvent &)
Function responsible for Remove and Publish PointCloud ()
ros::Publisher cloud_pc_laserScan