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
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef JSK_PCL_ROS_UTILS_POSE_WITH_COVARIANCE_STAMPED_TO_GAUSSIAN_POINTCLOUD_H_
00038 #define JSK_PCL_ROS_UTILS_POSE_WITH_COVARIANCE_STAMPED_TO_GAUSSIAN_POINTCLOUD_H_
00039
00040 #include <jsk_topic_tools/diagnostic_nodelet.h>
00041 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00042 #include <jsk_pcl_ros_utils/PoseWithCovarianceStampedToGaussianPointCloudConfig.h>
00043 #include <Eigen/Geometry>
00044 #include <dynamic_reconfigure/server.h>
00045 namespace jsk_pcl_ros_utils
00046 {
00047 class PoseWithCovarianceStampedToGaussianPointCloud: public jsk_topic_tools::DiagnosticNodelet
00048 {
00049 public:
00050 typedef PoseWithCovarianceStampedToGaussianPointCloudConfig Config;
00051 PoseWithCovarianceStampedToGaussianPointCloud():
00052 DiagnosticNodelet("PoseWithCovarianceStampedToGaussianPointCloud") {}
00053 protected:
00054 virtual void onInit();
00055 virtual void subscribe();
00056 virtual void unsubscribe();
00057 virtual void convert(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg);
00058 virtual void configCallback(Config& config, uint32_t level);
00059 virtual float gaussian(const Eigen::Vector2f& input,
00060 const Eigen::Vector2f& mean,
00061 const Eigen::Matrix2f& S,
00062 const Eigen::Matrix2f& S_inv);
00063 boost::mutex mutex_;
00064 ros::Publisher pub_;
00065 ros::Subscriber sub_;
00066 std::string cut_plane_;
00067 double threshold_;
00068 std::string normalize_method_;
00069 double normalize_value_;
00070 int sampling_num_;
00071 boost::shared_ptr<dynamic_reconfigure::Server<Config> > srv_;
00072 };
00073 }
00074
00075 #endif