convert_metric.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 #include <ros/ros.h>
35 #include <nodelet/nodelet.h>
38 #include <boost/thread.hpp>
39 
41 
43 
45 {
46  // Subscriptions
49 
50  // Publications
51  boost::mutex connect_mutex_;
53 
54  virtual void onInit();
55 
56  void connectCb();
57 
58  void depthCb(const sensor_msgs::ImageConstPtr& raw_msg);
59 };
60 
62 {
64  it_.reset(new image_transport::ImageTransport(nh));
65 
66  // Monitor whether anyone is subscribed to the output
68  // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_
69  boost::lock_guard<boost::mutex> lock(connect_mutex_);
70  pub_depth_ = it_->advertise("image", 1, connect_cb, connect_cb);
71 }
72 
73 // Handles (un)subscribing when clients (un)subscribe
75 {
76  boost::lock_guard<boost::mutex> lock(connect_mutex_);
77  if (pub_depth_.getNumSubscribers() == 0)
78  {
80  }
81  else if (!sub_raw_)
82  {
84  sub_raw_ = it_->subscribe("image_raw", 1, &ConvertMetricNodelet::depthCb, this, hints);
85  }
86 }
87 
88 void ConvertMetricNodelet::depthCb(const sensor_msgs::ImageConstPtr& raw_msg)
89 {
90  // Allocate new Image message
91  sensor_msgs::ImagePtr depth_msg( new sensor_msgs::Image );
92  depth_msg->header = raw_msg->header;
93  depth_msg->height = raw_msg->height;
94  depth_msg->width = raw_msg->width;
95 
96  // Set data, encoding and step after converting the metric.
97  if (raw_msg->encoding == enc::TYPE_16UC1)
98  {
99  depth_msg->encoding = enc::TYPE_32FC1;
100  depth_msg->step = raw_msg->width * (enc::bitDepth(depth_msg->encoding) / 8);
101  depth_msg->data.resize(depth_msg->height * depth_msg->step);
102  // Fill in the depth image data, converting mm to m
103  float bad_point = std::numeric_limits<float>::quiet_NaN ();
104  const uint16_t* raw_data = reinterpret_cast<const uint16_t*>(&raw_msg->data[0]);
105  float* depth_data = reinterpret_cast<float*>(&depth_msg->data[0]);
106  for (unsigned index = 0; index < depth_msg->height * depth_msg->width; ++index)
107  {
108  uint16_t raw = raw_data[index];
109  depth_data[index] = (raw == 0) ? bad_point : (float)raw * 0.001f;
110  }
111  }
112  else if (raw_msg->encoding == enc::TYPE_32FC1)
113  {
114  depth_msg->encoding = enc::TYPE_16UC1;
115  depth_msg->step = raw_msg->width * (enc::bitDepth(depth_msg->encoding) / 8);
116  depth_msg->data.resize(depth_msg->height * depth_msg->step);
117  // Fill in the depth image data, converting m to mm
118  uint16_t bad_point = 0;
119  const float* raw_data = reinterpret_cast<const float*>(&raw_msg->data[0]);
120  uint16_t* depth_data = reinterpret_cast<uint16_t*>(&depth_msg->data[0]);
121  for (unsigned index = 0; index < depth_msg->height * depth_msg->width; ++index)
122  {
123  float raw = raw_data[index];
124  depth_data[index] = std::isnan(raw) ? bad_point : (uint16_t)(raw * 1000);
125  }
126  }
127  else
128  {
129  ROS_ERROR("Unsupported image conversion from %s.", raw_msg->encoding.c_str());
130  return;
131  }
132 
133  pub_depth_.publish(depth_msg);
134 }
135 
136 } // namespace xiaoqiang_depth_image_proc
137 
138 // Register as nodelet
void depthCb(const sensor_msgs::ImageConstPtr &raw_msg)
boost::shared_ptr< image_transport::ImageTransport > it_
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
uint32_t getNumSubscribers() const
ros::NodeHandle & getPrivateNodeHandle() const
void publish(const sensor_msgs::Image &message) const
ros::NodeHandle & getNodeHandle() const
PLUGINLIB_EXPORT_CLASS(xiaoqiang_depth_image_proc::ConvertMetricNodelet, nodelet::Nodelet)
#define ROS_ERROR(...)


xiaoqiang_depth_image_proc
Author(s): Xie fusheng
autogenerated on Mon Jun 10 2019 15:53:04