intermittent_image_annotator_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 #define BOOST_PARAMETER_MAX_ARITY 7
38 #include <cv_bridge/cv_bridge.h>
42 #include <visualization_msgs/Marker.h>
44 #include <pcl/filters/extract_indices.h>
45 namespace jsk_pcl_ros
46 {
48  {
49  DiagnosticNodelet::onInit();
52  pnh_->param("fixed_frame_id", fixed_frame_id_, std::string("odom"));
53  pnh_->param("max_image_buffer", max_image_buffer_, 5);
54  pnh_->param("rate", rate_, 1.0);
55  pnh_->param("store_pointcloud", store_pointcloud_, false);
56  pnh_->param("keep_organized", keep_organized_, false);
57  pub_pose_ = pnh_->advertise<geometry_msgs::PoseStamped>(
58  "output/direction", 1);
59  pub_cloud_ = pnh_->advertise<sensor_msgs::PointCloud2>(
60  "output/cloud", 1);
61  pub_roi_ = pnh_->advertise<jsk_recognition_msgs::PosedCameraInfo>(
62  "output/roi", 1);
63  pub_marker_ = pnh_->advertise<visualization_msgs::Marker>(
64  "output/marker", 1);
65  // resize ring buffer
66  snapshot_buffer_ = boost::circular_buffer<SnapshotInformation::Ptr>(
69  image_pub_ = it.advertise("output", 1);
70  image_sub_ = it.subscribeCamera(
71  "input/image", 1,
73  this);
74  rect_sub_ = pnh_->subscribe("output/screenrectangle", 1,
76  this);
77  if (store_pointcloud_) {
78  cloud_sub_ = pnh_->subscribe("input/cloud", 1,
80  this);
81  }
82  shutter_service_ = pnh_->advertiseService(
83  "shutter",
85  request_service_ = pnh_->advertiseService(
86  "request",
88  clear_service_ = pnh_->advertiseService(
89  "clear",
92  }
93 
94  // we donnot use subscribe/unsubscribe on demand
96  {
97  }
98 
100  {
101  }
102 
104  const geometry_msgs::PolygonStamped::ConstPtr& rect)
105  {
106  boost::mutex::scoped_lock lock(mutex_);
107  int x0 = rect->polygon.points[0].x;
108  int x1 = rect->polygon.points[2].x;
109  int y0 = rect->polygon.points[0].y;
110  int y1 = rect->polygon.points[2].y;
111  if (x0 > x1) {
112  std::swap(x0, x1);
113  }
114  if (y0 > y1) {
115  std::swap(y0, y1);
116  }
117  // check x region
118  int width = latest_image_msg_->width;
119  int x0_index = x0 / width;
120  int x1_index = x1 / width;
121  if (x0_index != x1_index) {
122  NODELET_WARN("malformed rectangle");
123  return;
124  }
125  if (snapshot_buffer_.size() == 0) {
126  NODELET_WARN("Size of snapshot buffer is 0.");
127  return;
128  }
129  else {
130  int image_index = x0_index;
131  NODELET_INFO("image index: %d", image_index);
133  // local point
134  int width_offset = width * image_index;
135  int x0_wo_offset = x0 - width_offset;
136  int x1_wo_offset = x1 - width_offset;
137  cv::Point2d mid((x0_wo_offset + x1_wo_offset) / 2.0,
138  (y0 + y1) / 2.0);
139  Eigen::Affine3d pose(info->camera_pose_);
140  image_geometry::PinholeCameraModel camera_model = info->camera_;
141  cv::Point3d mid_3d = camera_model.projectPixelTo3dRay(mid);
142  Eigen::Vector3d ray(mid_3d.x, mid_3d.y, mid_3d.z); // ray is camera local
143  ray = ray / ray.norm();
144  Eigen::Vector3d ray_global = pose.rotation() * ray;
145  NODELET_INFO("ray: [%f, %f, %f]", ray_global[0], ray_global[1], ray_global[2]);
146 
147  Eigen::Vector3d z = pose.rotation() * Eigen::Vector3d::UnitZ();
148  NODELET_INFO("z: [%f, %f, %f]", z[0], z[1], z[2]);
149  Eigen::Vector3d original_pos = pose.translation();
150  Eigen::Quaterniond q;
151  q.setFromTwoVectors(z, ray_global);
152  NODELET_INFO("q: [%f, %f, %f, %f]", q.x(), q.y(), q.z(), q.w());
153  Eigen::Affine3d output_pose = pose.rotate(q);
154  output_pose.translation() = original_pos;
155  geometry_msgs::PoseStamped ros_pose;
156  tf::poseEigenToMsg(output_pose, ros_pose.pose);
157  ros_pose.header.stamp = latest_image_msg_->header.stamp;
158  ros_pose.header.frame_id = fixed_frame_id_;
159  pub_pose_.publish(ros_pose);
160 
161  // publish ROI
162  jsk_recognition_msgs::PosedCameraInfo camera_info;
163  camera_info.header.stamp = latest_image_msg_->header.stamp;
164  camera_info.header.frame_id = fixed_frame_id_;
165  camera_info.camera_info
166  = sensor_msgs::CameraInfo(info->camera_.cameraInfo());
167  camera_info.camera_info.roi.x_offset = x0_wo_offset;
168  camera_info.camera_info.roi.y_offset = y0;
169  camera_info.camera_info.roi.width = x1_wo_offset - x0_wo_offset;
170  camera_info.camera_info.roi.height = y1 - y0;
171  tf::poseEigenToMsg(info->camera_pose_, camera_info.offset);
172  pub_roi_.publish(camera_info);
173 
174  // marker
175  // 2d points
176  cv::Point2d A(x0_wo_offset, y0);
177  cv::Point2d B(x0_wo_offset, y1);
178  cv::Point2d C(x1_wo_offset, y1);
179  cv::Point2d D(x1_wo_offset, y0);
180  // 3d local points
181  cv::Point3d A_3d = info->camera_.projectPixelTo3dRay(A) * 3;
182  cv::Point3d B_3d = info->camera_.projectPixelTo3dRay(B) * 3;
183  cv::Point3d C_3d = info->camera_.projectPixelTo3dRay(C) * 3;
184  cv::Point3d D_3d = info->camera_.projectPixelTo3dRay(D) * 3;
185  cv::Point3d O_3d;
186  // convert to ros point
187  geometry_msgs::Point A_ros, B_ros, C_ros, D_ros, O_ros;
188  jsk_recognition_utils::pointFromXYZToXYZ<cv::Point3d, geometry_msgs::Point>(A_3d, A_ros);
189  jsk_recognition_utils::pointFromXYZToXYZ<cv::Point3d, geometry_msgs::Point>(B_3d, B_ros);
190  jsk_recognition_utils::pointFromXYZToXYZ<cv::Point3d, geometry_msgs::Point>(C_3d, C_ros);
191  jsk_recognition_utils::pointFromXYZToXYZ<cv::Point3d, geometry_msgs::Point>(D_3d, D_ros);
192  jsk_recognition_utils::pointFromXYZToXYZ<cv::Point3d, geometry_msgs::Point>(O_3d, O_ros);
193  // build edges
194  visualization_msgs::Marker marker;
195  marker.header.stamp = latest_image_msg_->header.stamp;
196  marker.header.frame_id = fixed_frame_id_;
197  tf::poseEigenToMsg(info->camera_pose_, marker.pose);
198  marker.type = visualization_msgs::Marker::LINE_LIST;
199  marker.points.push_back(O_ros); marker.points.push_back(A_ros);
200  marker.points.push_back(O_ros); marker.points.push_back(B_ros);
201  marker.points.push_back(O_ros); marker.points.push_back(C_ros);
202  marker.points.push_back(O_ros); marker.points.push_back(D_ros);
203  marker.points.push_back(A_ros); marker.points.push_back(B_ros);
204  marker.points.push_back(B_ros); marker.points.push_back(C_ros);
205  marker.points.push_back(C_ros); marker.points.push_back(D_ros);
206  marker.points.push_back(D_ros); marker.points.push_back(A_ros);
207  marker.scale.x = 0.01;
208  marker.scale.y = 0.01;
209  marker.scale.z = 0.01;
210  marker.color.a = 1.0;
211  marker.color.r = 1.0;
212  pub_marker_.publish(marker);
213 
214  // crop pointcloud
215  if (store_pointcloud_) {
216  publishCroppedPointCloud(info->cloud_,
217  A_3d, B_3d, C_3d, D_3d,
218  info->camera_pose_);
219  }
220  }
221  }
222 
223 
225  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
226  const cv::Point3d& A, const cv::Point3d& B,
227  const cv::Point3d& C, const cv::Point3d& D,
228  const Eigen::Affine3d& pose)
229  {
230  Eigen::Vector3f A_eigen, B_eigen, C_eigen, D_eigen;
231  pointFromXYZToVector<cv::Point3d, Eigen::Vector3f>(
232  A, A_eigen);
233  pointFromXYZToVector<cv::Point3d, Eigen::Vector3f>(
234  B, B_eigen);
235  pointFromXYZToVector<cv::Point3d, Eigen::Vector3f>(
236  C, C_eigen);
237  pointFromXYZToVector<cv::Point3d, Eigen::Vector3f>(
238  D, D_eigen);
239  Eigen::Affine3f posef;
240  convertEigenAffine3(pose, posef);
241  Eigen::Vector3f A_global = posef * A_eigen;
242  Eigen::Vector3f B_global = posef * B_eigen;
243  Eigen::Vector3f C_global = posef * C_eigen;
244  Eigen::Vector3f D_global = posef * D_eigen;
245  Eigen::Vector3f O_global = posef.translation();
246  jsk_recognition_utils::Vertices vertices0, vertices1, vertices2, vertices3;
247  vertices0.push_back(O_global); vertices0.push_back(A_global); vertices0.push_back(D_global);
248  vertices1.push_back(O_global); vertices1.push_back(B_global); vertices1.push_back(A_global);
249  vertices2.push_back(O_global); vertices2.push_back(C_global); vertices2.push_back(B_global);
250  vertices3.push_back(O_global); vertices3.push_back(D_global); vertices3.push_back(C_global);
251  Polygon::Ptr plane0 (new Polygon(vertices0));
252  Polygon::Ptr plane1 (new Polygon(vertices1));
253  Polygon::Ptr plane2 (new Polygon(vertices2));
254  Polygon::Ptr plane3 (new Polygon(vertices3));
255  pcl::PointIndices::Ptr indices(new pcl::PointIndices);
256  for (size_t i = 0; i < cloud->points.size(); i++) {
257  pcl::PointXYZRGB p = cloud->points[i];
258  Eigen::Vector3f pf = p.getVector3fMap();
259  if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) {
260  if (plane0->signedDistanceToPoint(pf) > 0 &&
261  plane1->signedDistanceToPoint(pf) > 0 &&
262  plane2->signedDistanceToPoint(pf) > 0 &&
263  plane3->signedDistanceToPoint(pf) > 0) {
264  indices->indices.push_back(i);
265  }
266  }
267  }
268  pcl::ExtractIndices<pcl::PointXYZRGB> ex;
269  pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
270  ex.setInputCloud(cloud);
271  ex.setKeepOrganized(keep_organized_);
272  ex.setIndices(indices);
273  ex.filter(*output_cloud);
274  sensor_msgs::PointCloud2 ros_cloud;
275  pcl::toROSMsg(*output_cloud, ros_cloud);
276  ros_cloud.header.stamp = latest_image_msg_->header.stamp;
277  ros_cloud.header.frame_id = fixed_frame_id_;
278  pub_cloud_.publish(ros_cloud);
279  }
280 
282  const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
283  {
284  boost::mutex::scoped_lock lock(mutex_);
285  latest_cloud_msg_ = cloud_msg;
286  }
287 
289  const sensor_msgs::Image::ConstPtr& image_msg,
290  const sensor_msgs::CameraInfo::ConstPtr& info_msg)
291  {
292  boost::mutex::scoped_lock lock(mutex_);
293  vital_checker_->poke();
294  latest_image_msg_ = image_msg;
295  latest_camera_info_msg_ = info_msg;
296 
297  if (snapshot_buffer_.size() != 0) {
299  if ((now - last_publish_time_).toSec() > 1.0 / rate_) {
300  cv::Mat concatenated_image;
301  std::vector<cv::Mat> images;
302  //ROS_INFO("%lu images", snapshot_buffer_.size());
303  for (size_t i = 0; i < snapshot_buffer_.size(); i++) {
304  images.push_back(snapshot_buffer_[i]->image_);
305  }
306  cv::hconcat(images, concatenated_image);
307  cv_bridge::CvImage concatenate_bridge(latest_camera_info_msg_->header, // ??
309  concatenated_image);
310  image_pub_.publish(concatenate_bridge.toImageMsg());
312  }
313  }
314 
315  }
316 
318  {
320  ros::Rate r(10);
321  while (ros::ok()) {
322  {
323  boost::mutex::scoped_lock lock(mutex_);
324  if (latest_image_msg_ && latest_image_msg_->header.stamp > now) {
325  return;
326  }
327  }
328  r.sleep();
329  }
330  }
331 
333  std_srvs::Empty::Request& req,
334  std_srvs::Empty::Response& res)
335  {
336 
338  {
339  boost::mutex::scoped_lock lock(mutex_);
342  info (new SnapshotInformation());
343  // resolve tf
344  try {
347  latest_camera_info_msg_->header.frame_id,
348  latest_camera_info_msg_->header.stamp,
349  ros::Duration(1.0))) {
350  tf::StampedTransform transform;
352  latest_camera_info_msg_->header.frame_id,
353  latest_camera_info_msg_->header.stamp,
354  transform);
358  Eigen::Affine3d eigen_transform;
361  tf::transformTFToEigen(transform, eigen_transform);
362  info->camera_pose_ = eigen_transform;
363  info->camera_ = camera;
364  info->image_ = cv_ptr->image;
365  if (store_pointcloud_) {
366  // use pointcloud
367  if (!latest_cloud_msg_) {
368  NODELET_ERROR("no pointcloud is available");
369  return false;
370  }
371  // transform pointcloud to fixed frame
372  pcl::PointCloud<pcl::PointXYZRGB>::Ptr
373  nontransformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
374  pcl::PointCloud<pcl::PointXYZRGB>::Ptr
375  transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
376  pcl::fromROSMsg(*latest_cloud_msg_, *nontransformed_cloud);
378  *nontransformed_cloud,
379  *transformed_cloud,
380  *listener_)) {
381  info->cloud_ = transformed_cloud;
382  }
383  else {
384  NODELET_ERROR("failed to transform pointcloud");
385  return false;
386  }
387  }
388  snapshot_buffer_.push_back(info);
389  return true;
390  }
391  else {
392  NODELET_ERROR("failed to resolve tf from %s to %s",
393  fixed_frame_id_.c_str(),
394  latest_camera_info_msg_->header.frame_id.c_str());
395  return false;
396  }
397  }
398  catch (tf2::ConnectivityException &e)
399  {
400  NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
401  return false;
402  }
404  {
405  NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
406  return false;
407  }
408  }
409  else {
410  NODELET_ERROR("not yet camera message is available");
411  return false;
412  }
413  }
414  }
415 
417  std_srvs::Empty::Request& req,
418  std_srvs::Empty::Response& res)
419  {
420  boost::mutex::scoped_lock lock(mutex_);
421  snapshot_buffer_.clear();
422  return true;
423  }
424 
426  std_srvs::Empty::Request& req,
427  std_srvs::Empty::Response& res)
428  {
429  // concatenate images
430  boost::mutex::scoped_lock lock(mutex_);
431  if (snapshot_buffer_.size() == 0) {
432  NODELET_ERROR("no image is stored");
433  return false;
434  }
435  else {
436  cv::Mat concatenated_image;
437  std::vector<cv::Mat> images;
438  ROS_INFO("%lu images", snapshot_buffer_.size());
439  for (size_t i = 0; i < snapshot_buffer_.size(); i++) {
440  images.push_back(snapshot_buffer_[i]->image_);
441  }
442  cv::hconcat(images, concatenated_image);
443  cv_bridge::CvImage concatenate_bridge(latest_camera_info_msg_->header, // ??
445  concatenated_image);
446  image_pub_.publish(concatenate_bridge.toImageMsg());
447  return true;
448  }
449  }
450 }
451 
virtual void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
virtual void rectCallback(const geometry_msgs::PolygonStamped::ConstPtr &rect)
info
#define NODELET_ERROR(...)
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
cv::Point3d projectPixelTo3dRay(const cv::Point2d &uv_rect) const
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
boost::circular_buffer< SnapshotInformation::Ptr > snapshot_buffer_
pose
D
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
q
sensor_msgs::PointCloud2::ConstPtr latest_cloud_msg_
void publish(const sensor_msgs::Image &message) const
#define ROS_INFO(...)
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())
ROSCPP_DECL bool ok()
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::IntermittentImageAnnotator, nodelet::Nodelet)
void convertEigenAffine3(const Eigen::Affine3d &from, Eigen::Affine3f &to)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
bool sleep()
B
jsk_topic_tools::VitalChecker::Ptr vital_checker_
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
virtual bool shutterCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
C
#define NODELET_INFO(...)
virtual void cameraCallback(const sensor_msgs::Image::ConstPtr &image_msg, const sensor_msgs::CameraInfo::ConstPtr &info_msg)
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
p
virtual void publishCroppedPointCloud(pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud, const cv::Point3d &A, const cv::Point3d &B, const cv::Point3d &C, const cv::Point3d &D, const Eigen::Affine3d &pose)
static Time now()
static tf::TransformListener * getInstance()
sensor_msgs::CameraInfo::ConstPtr latest_camera_info_msg_
virtual bool requestCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
sensor_msgs::ImagePtr toImageMsg() const
width
virtual bool clearCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)


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