Go to the documentation of this file.
37 #ifndef JSK_PCL_ROS_HEIGHTMAP_TIME_ACCUMULATION_H_
38 #define JSK_PCL_ROS_HEIGHTMAP_TIME_ACCUMULATION_H_
40 #include <jsk_topic_tools/connection_based_nodelet.h>
41 #include <jsk_recognition_msgs/HeightmapConfig.h>
42 #include <jsk_pcl_ros/HeightmapTimeAccumulationConfig.h>
43 #include <dynamic_reconfigure/server.h>
44 #include <sensor_msgs/PointCloud2.h>
45 #include <sensor_msgs/Image.h>
46 #include <opencv2/opencv.hpp>
52 #include <std_srvs/Empty.h>
55 #include <boost/accumulators/accumulators.hpp>
56 #include <boost/accumulators/statistics/mean.hpp>
57 #include <boost/accumulators/statistics/variance.hpp>
58 #include <boost/accumulators/statistics/count.hpp>
59 #include <boost/accumulators/statistics/stats.hpp>
65 class HeightmapTimeAccumulation:
66 public jsk_topic_tools::ConnectionBasedNodelet
70 typedef HeightmapTimeAccumulationConfig
Config;
71 typedef boost::accumulators::accumulator_set<
83 const sensor_msgs::Image::ConstPtr& new_heightmap);
85 const cv::Mat& heightmap,
const std_msgs::Header&
header);
87 virtual bool isValidIndex(
const cv::Point& index,
const cv::Mat& map);
88 virtual bool isValidCell(
const cv::Point& index,
const cv::Mat& map);
90 const jsk_recognition_msgs::HeightmapConfig::ConstPtr& config);
92 const sensor_msgs::PointCloud2::ConstPtr&
msg);
94 std_srvs::Empty::Response&
res);
96 cv::Mat &new_heightmap);
98 cv::Mat &new_heightmap);
116 jsk_recognition_msgs::HeightmapConfig::ConstPtr
config_;
ros::ServiceServer srv_reset_
Eigen::Affine3f prev_from_center_to_fixed_
virtual bool isValidCell(const cv::Point &index, const cv::Mat &map)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
std::vector< OneDataStat::Ptr > stats
global variable to store stats.
boost::accumulators::accumulator_set< float, boost::accumulators::stats< boost::accumulators::tag::variance, boost::accumulators::tag::count, boost::accumulators::tag::mean > > Accumulator
tf::TransformListener * tf_
virtual void unsubscribe()
virtual void configCallback(Config &config, uint32_t level)
virtual cv::Point toIndex(const PointType &p, const cv::Mat &map)
ros::Publisher pub_output_
ros::Publisher pub_config_
boost::shared_ptr< HeightmapTimeAccumulation > Ptr
boost::shared_ptr< tf::MessageFilter< sensor_msgs::Image > > tf_filter_
virtual bool resetCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
virtual void configTopicCallback(const jsk_recognition_msgs::HeightmapConfig::ConstPtr &config)
jsk_recognition_msgs::HeightmapConfig::ConstPtr config_
HeightmapTimeAccumulationConfig Config
pcl::PointCloud< PointType > prev_cloud_
message_filters::Subscriber< sensor_msgs::Image > sub_heightmap_
ros::Subscriber sub_previous_pointcloud_
double count(const OneDataStat &d)
wrapper function for count method for boost::function
int bilateral_filter_size_
std::string fixed_frame_id_
virtual bool isValidIndex(const cv::Point &index, const cv::Mat &map)
double bilateral_sigma_color_
HeightmapTimeAccumulation()
virtual void accumulate(const sensor_msgs::Image::ConstPtr &new_heightmap)
double bilateral_sigma_space_
std::string center_frame_id_
virtual void mergedAccmulation(pcl::PointCloud< PointType > &transformed_pointcloud, cv::Mat &new_heightmap)
ros::Subscriber sub_config_
cv::Mat accumulated_heightmap_
boost::mutex mutex
global mutex.
virtual void publishHeightmap(const cv::Mat &heightmap, const std_msgs::Header &header)
virtual void prevPointCloud(const sensor_msgs::PointCloud2::ConstPtr &msg)
virtual void overwriteAccmulation(pcl::PointCloud< PointType > &transformed_pointcloud, cv::Mat &new_heightmap)
jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jan 7 2025 04:05:44