27 #ifndef _LASER3D_POINTCLOUD_H_
28 #define _LASER3D_POINTCLOUD_H_
39 #include <laser_geometry/laser_geometry.h>
40 #include <sensor_msgs/LaserScan.h>
41 #include <sensor_msgs/JointState.h>
42 #include <sensor_msgs/PointCloud2.h>
43 #include <tf/transform_listener.h>
44 #include <pcl_ros/transforms.h>
45 #include <pcl/conversions.h>
46 #include <pcl_conversions/pcl_conversions.h>
47 #include <pcl/point_cloud.h>
48 #include <pcl/point_types.h>
53 #include <boost/numeric/ublas/matrix.hpp>
54 #include <boost/thread/mutex.hpp>
63 namespace channel_option
105 std::vector<sensor_msgs::JointStatePtr>
joints;
114 std::vector<sensor_msgs::LaserScanPtr>
scans;
128 max_scans_accumulated=450;
134 laserscan_arrived=
false;
146 void accumulate_cloud(pcl::PointCloud<pcl::PointXYZ>* pc_in);
173 const sensor_msgs::LaserScan &scan_in,
174 sensor_msgs::PointCloud2 &cloud_out,
175 const sensor_msgs::JointState &state_start,
176 const sensor_msgs::JointState &state_end,
178 double range_cutoff = -1.0,
182 error=las3D_transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, state_start,state_end, tf, range_cutoff, channel_options);
208 const sensor_msgs::LaserScan &scan_in,
209 sensor_msgs::PointCloud2 &cloud_out,
211 double range_cutoff = -1.0,
215 error=las3D_transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options);
227 const boost::numeric::ublas::matrix<double>& getUnitVectors_(
double angle_min,
229 double angle_increment,
230 unsigned int length);
236 void las3D_projectLaser_ (
const sensor_msgs::LaserScan& scan_in,
237 sensor_msgs::PointCloud2 &cloud_out,
239 int channel_options);
243 int las3D_transformLaserScanToPointCloud_ (
const std::string &target_frame,
244 const sensor_msgs::LaserScan &scan_in,
245 sensor_msgs::PointCloud2 &cloud_out,
246 const sensor_msgs::JointState &state_start,
247 const sensor_msgs::JointState &state_end,
250 int channel_options);
252 int las3D_transformLaserScanToPointCloud_ (
const std::string &target_frame,
253 const sensor_msgs::LaserScan &scan_in,
254 sensor_msgs::PointCloud2 &cloud_out,
257 int channel_options);
int accumulation_mode
Type of accumulation wanted to be used.
const float LASER_SCAN_MAX_RANGE
pcl::PointCloud< pcl::PointXYZ > pc_accumulated
PointCloud<pcl::PointXYZ> to accumulate the laser scans.
int laser_count
Number of laser scans received.
const float LASER_SCAN_MIN_RANGE
Eigen::ArrayXXd co_sine_map_
std::map< std::string, boost::numeric::ublas::matrix< double > * > unit_vector_map_
Internal map of pointers to stored values.
int pointcloud_stamp
Time stamp to the output cloud, when using rosbag or in real time accumulation.
int las3D_transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, tf::Transformer &tf, double range_cutoff=-1.0, int channel_options=channel_option::Default)
Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame using the scan tim...
std::vector< sensor_msgs::JointStatePtr > joints
Vector with the external shaft angle mensage.
ChannelOption
Enumerated output channels options.
const float LASER_SCAN_INVALID
Enable "intensities" and "index" channels.
Enable "intensities" channel.
Enable "distances" channel.
Enable "viewpoint" channel.
tf::TransformListener * p_listener
ROS tf listener.
std::vector< sensor_msgs::LaserScanPtr > scans
Vector with the laser scans received.
Class to accumulate point clouds laser3D.
bool laserscan_arrived
bool laser scan received
size_t cloud_size
PointCloud size.
int max_scans_accumulated
Max of scans to be accumulated.
int las3D_transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, const sensor_msgs::JointState &state_start, const sensor_msgs::JointState &state_end, tf::Transformer &tf, double range_cutoff=-1.0, int channel_options=channel_option::Default)
Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame using the external...