36 #define BOOST_PARAMETER_MAX_ARITY 7 41 #include <opencv2/opencv.hpp> 49 DiagnosticNodelet::onInit();
50 pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
55 pub_cloud_ = advertise<sensor_msgs::PointCloud2>(*
pnh_,
"output/cloud", 1);
66 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
75 "input/camera_info", 1,
96 const sensor_msgs::CameraInfo::ConstPtr& camera_info_msg)
101 cv::Mat image = cv_ptr->image;
102 cv::Rect roi(camera_info_msg->roi.x_offset, camera_info_msg->roi.y_offset,
103 camera_info_msg->roi.width, camera_info_msg->roi.height);
105 cv::Mat image_roi = image(roi);
121 const sensor_msgs::CameraInfo::ConstPtr& info_msg)
128 const sensor_msgs::Image::ConstPtr& image_msg)
137 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
145 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
cloud 146 (
new pcl::PointCloud<pcl::PointXYZRGB>);
148 pcl::PointCloud<pcl::PointXYZRGB>::Ptr clipped_cloud
149 (
new pcl::PointCloud<pcl::PointXYZRGB>);
151 pcl::PointXYZRGB nan_point;
152 nan_point.x = nan_point.y = nan_point.z
153 = std::numeric_limits<float>::quiet_NaN();;
154 for (
size_t i = 0; i < cloud->points.size(); i++) {
155 pcl::PointXYZRGB
p = cloud->points[i];
157 if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) {
159 if (uv.x >= 0 && uv.x <= region.width &&
160 uv.y >= 0 && uv.y <= region.height) {
161 indices.indices.push_back(i);
162 clipped_cloud->points.push_back(p);
167 clipped_cloud->points.push_back(nan_point);
171 clipped_cloud->width = cloud->width;
172 clipped_cloud->height = cloud->height;
174 sensor_msgs::PointCloud2 ros_cloud;
176 ros_cloud.header = cloud_msg->header;
178 indices.header = cloud_msg->header;
virtual void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::ROIClipper, nodelet::Nodelet)
sensor_msgs::CameraInfo::ConstPtr latest_camera_info_
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual void infoCallback(const sensor_msgs::CameraInfo::ConstPtr &info_msg)
ros::Publisher pub_image_
virtual void imageCallback(const sensor_msgs::Image::ConstPtr &image_msg)
ros::Subscriber sub_image_no_sync_
ros::Publisher pub_cloud_indices_
message_filters::Subscriber< sensor_msgs::Image > sub_image_
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
virtual void clip(const sensor_msgs::Image::ConstPtr &image_msg, const sensor_msgs::CameraInfo::ConstPtr &camera_info_msg)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
ros::Subscriber sub_cloud_no_sync_
ros::Publisher pub_cloud_
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
virtual void unsubscribe()
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_info_
cv::Point2d project3dToPixel(const cv::Point3d &xyz) const
pcl::PointIndices PCLIndicesMsg
ros::Subscriber sub_info_no_sync_
cv::Rect rectifiedRoi() const
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
sensor_msgs::ImagePtr toImageMsg() const