36 #ifndef JSK_PCL_ROS_HEIGHTMAP_CONVERTER_H_ 37 #define JSK_PCL_ROS_HEIGHTMAP_CONVERTER_H_ 41 #include <sensor_msgs/PointCloud2.h> 42 #include <jsk_pcl_ros/HeightmapConverterConfig.h> 43 #include <dynamic_reconfigure/server.h> 44 #include <opencv2/opencv.hpp> 45 #include <pcl/point_types.h> 46 #include <pcl/point_cloud.h> 47 #include <jsk_recognition_msgs/HeightmapConfig.h> 58 typedef HeightmapConverterConfig
Config;
65 virtual void convert(
const sensor_msgs::PointCloud2::ConstPtr&
msg);
66 inline cv::Point
toIndex(
const pcl::PointXYZ& p)
const 70 return cv::Point(-1, -1);
72 const float offsetted_x = p.x -
min_x_;
73 const float offsetted_y = p.y -
min_y_;
76 return cv::Point(std::floor(offsetted_x / dx),
77 std::floor(offsetted_y / dy));
HeightmapConverterConfig Config
bool use_projected_center_
tf::TransformListener * tf_
ros::Publisher pub_config_
virtual void unsubscribe()
double duration_transform_timeout_
tf::TransformBroadcaster tf_broadcaster_
std::string center_frame_id_
cv::Point toIndex(const pcl::PointXYZ &p) const
std::string fixed_frame_id_
std::string projected_center_frame_id_
virtual void convert(const sensor_msgs::PointCloud2::ConstPtr &msg)
boost::shared_ptr< HeightmapConverter > Ptr
boost::mutex mutex
global mutex.
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void configCallback(Config &config, uint32_t level)
double initial_probability_