37 #ifndef JSK_PCL_ROS_UTILS_COLORIZE_DISTANCE_FROM_PLANE_H_ 38 #define JSK_PCL_ROS_UTILS_COLORIZE_DISTANCE_FROM_PLANE_H_ 45 #include <sensor_msgs/PointCloud2.h> 46 #include <jsk_recognition_msgs/ClusterPointIndices.h> 47 #include <jsk_recognition_msgs/ModelCoefficientsArray.h> 48 #include <jsk_recognition_msgs/PolygonArray.h> 49 #include <dynamic_reconfigure/server.h> 50 #include <jsk_pcl_ros_utils/ColorizeDistanceFromPlaneConfig.h> 62 sensor_msgs::PointCloud2,
63 jsk_recognition_msgs::ModelCoefficientsArray,
64 jsk_recognition_msgs::PolygonArray
66 typedef ColorizeDistanceFromPlaneConfig
Config;
73 virtual void colorize(
const sensor_msgs::PointCloud2::ConstPtr&
cloud,
74 const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients,
75 const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons);
78 const PointT& p,
const std::vector<jsk_recognition_utils::ConvexPolygon::Ptr>& convexes);
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, jsk_recognition_msgs::ModelCoefficientsArray, jsk_recognition_msgs::PolygonArray > SyncPolicy
boost::shared_ptr< ColorizeDistanceFromPlane > Ptr
message_filters::Subscriber< jsk_recognition_msgs::ModelCoefficientsArray > sub_coefficients_
virtual void configCallback(Config &config, uint32_t level)
virtual void colorize(const sensor_msgs::PointCloud2::ConstPtr &cloud, const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr &coefficients, const jsk_recognition_msgs::PolygonArray::ConstPtr &polygons)
message_filters::Subscriber< jsk_recognition_msgs::PolygonArray > sub_polygons_
ColorizeDistanceFromPlaneConfig Config
virtual void unsubscribe()
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual uint32_t colorForDistance(const double d)
virtual double distanceToConvexes(const PointT &p, const std::vector< jsk_recognition_utils::ConvexPolygon::Ptr > &convexes)