Go to the documentation of this file.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 #ifndef _trajectory_3Dscans_2_pointcloud_alg_node_h_
00026 #define _trajectory_3Dscans_2_pointcloud_alg_node_h_
00027
00028 #include <iri_base_algorithm/iri_base_algorithm.h>
00029 #include "trajectory_3Dscans_2_pointcloud_alg.h"
00030 #include <Eigen/Dense>
00031
00032 #include <sensor_msgs/LaserScan.h>
00033 #include <sensor_msgs/PointCloud2.h>
00034
00035 #include <iri_poseslam/Trajectory.h>
00036
00037 #include <pcl/point_types.h>
00038 #include <pcl/point_cloud.h>
00039 #include <pcl_ros/point_cloud.h>
00040 #include <pcl/ros/conversions.h>
00041 #include <pcl_ros/transforms.h>
00042
00043 #include <laser_geometry/laser_geometry.h>
00044 #include <math.h>
00045
00046
00047
00048
00049
00050 using namespace Eigen;
00051
00056 class Trajectory3DScans2PointcloudAlgNode : public algorithm_base::IriBaseAlgorithm<Trajectory3DScans2PointcloudAlgorithm>
00057 {
00058 private:
00059
00060 Matrix4f T_laser_frame;
00061 bool emptyPointCloud_, started_;
00062
00063
00064 ros::Publisher slices3D_pointcloud_publisher_;
00065 sensor_msgs::PointCloud2 PointCloud_msg_;
00066
00067
00068 ros::Subscriber slices3D_subscriber_;
00069 void slices3D_callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
00070 ros::Subscriber trajectory_subscriber_;
00071 void trajectory_callback(const iri_poseslam::Trajectory::ConstPtr& msg);
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082 std::vector<sensor_msgs::PointCloud2> slice_buffer_;
00083 std::vector<sensor_msgs::PointCloud2> trajectory_slice_buffer_;
00084 std::vector<std::pair<uint, double> > interpolation_buffer_;
00085
00086 public:
00093 Trajectory3DScans2PointcloudAlgNode(void);
00094
00101 ~Trajectory3DScans2PointcloudAlgNode(void);
00102
00103 protected:
00116 void mainNodeThread(void);
00117
00130 void node_config_update(Config &config, uint32_t level);
00131
00138 void addNodeDiagnostics(void);
00139
00140
00141
00142
00149 void recompute_PointCloud_msg(const iri_poseslam::Trajectory& trajectory);
00150
00158 sensor_msgs::PointCloud2 transform_point_cloud(const sensor_msgs::PointCloud2& pcloud, const geometry_msgs::Pose& pose);
00159
00166 void add_to_PointCloud_msg(const sensor_msgs::PointCloud2& newPointCloud);
00167
00173 void clear_PointCloud_msg();
00174
00183 geometry_msgs::Pose interpole_slice_pose(const uint& id, const iri_poseslam::Trajectory& trajectory) const;
00184
00193 std::pair<uint,double> search_interpolation(const sensor_msgs::PointCloud2& slice, const iri_poseslam::Trajectory& trajectory) const;
00194
00203 Matrix4f transformation_matrix(const float x, const float y, const float z, const float alpha) const;
00204
00212 double pi_2_pi(const double& angle) const;
00213 };
00214
00215 #endif