roi_clipper_nodelet.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 #define BOOST_PARAMETER_MAX_ARITY 7
37 
39 #include <cv_bridge/cv_bridge.h>
41 #include <opencv2/opencv.hpp>
44 
45 namespace jsk_pcl_ros
46 {
48  {
49  DiagnosticNodelet::onInit();
50  pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
51  pnh_->param("not_sync", not_sync_, false);
52  pnh_->param("keep_organized", keep_organized_, false);
53  pub_image_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
54  if (not_sync_) {
55  pub_cloud_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output/cloud", 1);
56  pub_cloud_indices_ = advertise<PCLIndicesMsg>(*pnh_, "output/cloud_indices", 1);
57  }
59  }
60 
62  {
63  if (!not_sync_) {
64  sub_image_.subscribe(*pnh_, "input/image", 1);
65  sub_info_.subscribe(*pnh_, "input/camera_info", 1);
66  sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
67  sync_->connectInput(sub_image_, sub_info_);
68  sync_->registerCallback(boost::bind(&ROIClipper::clip, this, _1, _2));
69  }
70  else {
71  sub_image_no_sync_ = pnh_->subscribe(
72  "input/image", 1,
74  sub_info_no_sync_ = pnh_->subscribe(
75  "input/camera_info", 1,
77  sub_cloud_no_sync_ = pnh_->subscribe(
78  "input/cloud", 1,
80  }
81  }
82 
84  {
85  if (!not_sync_) {
88  }
89  else {
93  }
94  }
95  void ROIClipper::clip(const sensor_msgs::Image::ConstPtr& image_msg,
96  const sensor_msgs::CameraInfo::ConstPtr& camera_info_msg)
97  {
98  vital_checker_->poke();
99  try {
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);
104  //NODELET_INFO("roi::(%d, %d, %d, %d)", roi.x, roi.y, roi.width, roi.height);
105  cv::Mat image_roi = image(roi);
106  // cv::imshow("roi", image_roi);
107  // cv::waitKey(3);
108  cv_bridge::CvImage roi_bridge(image_msg->header,
110  image_roi);
111  pub_image_.publish(roi_bridge.toImageMsg());
112  }
113  catch (cv_bridge::Exception& e)
114  {
115  NODELET_ERROR("cv_bridge exception: %s", e.what());
116  return;
117  }
118  }
119 
121  const sensor_msgs::CameraInfo::ConstPtr& info_msg)
122  {
123  boost::mutex::scoped_lock lock(mutex_);
124  latest_camera_info_ = info_msg;
125  }
126 
128  const sensor_msgs::Image::ConstPtr& image_msg)
129  {
130  boost::mutex::scoped_lock lock(mutex_);
131  if (latest_camera_info_) {
132  clip(image_msg, latest_camera_info_);
133  }
134  }
135 
137  const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
138  {
139  boost::mutex::scoped_lock lock(mutex_);
140  vital_checker_->poke();
141  if (latest_camera_info_) {
143  PCLIndicesMsg indices;
145  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud
146  (new pcl::PointCloud<pcl::PointXYZRGB>);
147  pcl::fromROSMsg(*cloud_msg, *cloud);
148  pcl::PointCloud<pcl::PointXYZRGB>::Ptr clipped_cloud
149  (new pcl::PointCloud<pcl::PointXYZRGB>);
150  cv::Rect region = model.rectifiedRoi();
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];
156  bool foundp = false;
157  if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) {
158  cv::Point2d uv = model.project3dToPixel(cv::Point3d(p.x, p.y, 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);
163  foundp = true;
164  }
165  }
166  if (!foundp && keep_organized_) {
167  clipped_cloud->points.push_back(nan_point);
168  }
169  }
170  if (keep_organized_) {
171  clipped_cloud->width = cloud->width;
172  clipped_cloud->height = cloud->height;
173  }
174  sensor_msgs::PointCloud2 ros_cloud;
175  pcl::toROSMsg(*clipped_cloud, ros_cloud);
176  ros_cloud.header = cloud_msg->header;
177  pub_cloud_.publish(ros_cloud);
178  indices.header = cloud_msg->header;
179  pub_cloud_indices_.publish(indices);
180  }
181  }
182 }
183 
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_
Definition: roi_clipper.h:84
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_
Definition: roi_clipper.h:75
virtual void imageCallback(const sensor_msgs::Image::ConstPtr &image_msg)
ros::Subscriber sub_image_no_sync_
Definition: roi_clipper.h:81
ros::Publisher pub_cloud_indices_
Definition: roi_clipper.h:77
message_filters::Subscriber< sensor_msgs::Image > sub_image_
Definition: roi_clipper.h:78
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
boost::shared_ptr< ros::NodeHandle > pnh_
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)
jsk_topic_tools::VitalChecker::Ptr vital_checker_
ros::Subscriber sub_cloud_no_sync_
Definition: roi_clipper.h:83
ros::Publisher pub_cloud_
Definition: roi_clipper.h:76
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)
p
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_info_
Definition: roi_clipper.h:79
cv::Point2d project3dToPixel(const cv::Point3d &xyz) const
pcl::PointIndices PCLIndicesMsg
cloud
ros::Subscriber sub_info_no_sync_
Definition: roi_clipper.h:82
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
Definition: roi_clipper.h:80
sensor_msgs::ImagePtr toImageMsg() const


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:47