image_rotate_nodelet.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, JSK Lab.
5 * 2008, Willow Garage, Inc.
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/or other materials provided
17 * with the distribution.
18 * * Neither the name of the Willow Garage 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 /********************************************************************
37 * image_rotate_nodelet.cpp
38 * this is a forked version of image_rotate.
39 * this image_rotate_nodelet supports:
40 * 1) nodelet
41 * 2) tf and tf2
42 *********************************************************************/
43 
44 #include <ros/ros.h>
45 #include <nodelet/nodelet.h>
48 #include <image_rotate/ImageRotateConfig.h>
49 #include <geometry_msgs/Vector3Stamped.h>
51 #include <cv_bridge/cv_bridge.h>
52 #include <opencv2/imgproc/imgproc.hpp>
53 #include <dynamic_reconfigure/server.h>
54 #include <math.h>
56 
57 namespace image_rotate {
59 {
63 
64  image_rotate::ImageRotateConfig config_;
65  dynamic_reconfigure::Server<image_rotate::ImageRotateConfig> srv;
66 
70 
71  geometry_msgs::Vector3Stamped target_vector_;
72  geometry_msgs::Vector3Stamped source_vector_;
73 
76 
78  double angle_;
80 
81  void reconfigureCallback(image_rotate::ImageRotateConfig &new_config, uint32_t level)
82  {
83  config_ = new_config;
84  target_vector_.vector.x = config_.target_x;
85  target_vector_.vector.y = config_.target_y;
86  target_vector_.vector.z = config_.target_z;
87 
88  source_vector_.vector.x = config_.source_x;
89  source_vector_.vector.y = config_.source_y;
90  source_vector_.vector.z = config_.source_z;
91  if (subscriber_count_)
92  { // @todo Could do this without an interruption at some point.
93  unsubscribe();
94  subscribe();
95  }
96  }
97 
98  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
99  {
100  if (frame.empty())
101  return image_frame;
102  return frame;
103  }
104 
105  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
106  {
107  do_work(msg, cam_info->header.frame_id);
108  }
109 
110  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
111  {
112  do_work(msg, msg->header.frame_id);
113  }
114 
115  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
116  {
117  try
118  {
119  std::string input_frame_id = frameWithDefault(config_.input_frame_id, input_frame_from_msg);
120  std::string target_frame_id = frameWithDefault(config_.target_frame_id, input_frame_from_msg);
121  std::string source_frame_id = frameWithDefault(config_.source_frame_id, input_frame_from_msg);
122 
123  // Transform the target vector into the image frame.
124  target_vector_.header.stamp = msg->header.stamp;
125  target_vector_.header.frame_id = target_frame_id;
126  geometry_msgs::Vector3Stamped target_vector_transformed;
127  geometry_msgs::TransformStamped transform = tf_buffer_.lookupTransform(target_frame_id, input_frame_id, msg->header.stamp, msg->header.stamp-prev_stamp_);
128  tf2::doTransform(target_vector_, target_vector_transformed, transform);
129 
130  // Transform the source vector into the image frame.
131  source_vector_.header.stamp = msg->header.stamp;
132  source_vector_.header.frame_id = source_frame_id;
133  geometry_msgs::Vector3Stamped source_vector_transformed;
134  transform = tf_buffer_.lookupTransform(source_frame_id, input_frame_id, msg->header.stamp, msg->header.stamp-prev_stamp_);
135  tf2::doTransform(source_vector_, source_vector_transformed, transform);
136 
137  //NODELET_INFO("target: %f %f %f", target_vector_.x(), target_vector_.y(), target_vector_.z());
138  //NODELET_INFO("target_transformed: %f %f %f", target_vector_transformed.x(), target_vector_transformed.y(), target_vector_transformed.z());
139  //NODELET_INFO("source: %f %f %f", source_vector_.x(), source_vector_.y(), source_vector_.z());
140  //NODELET_INFO("source_transformed: %f %f %f", source_vector_transformed.x(), source_vector_transformed.y(), source_vector_transformed.z());
141 
142  // Calculate the angle of the rotation.
143  double angle = angle_;
144  if ((target_vector_transformed.vector.x != 0 || target_vector_transformed.vector.y != 0) &&
145  (source_vector_transformed.vector.x != 0 || source_vector_transformed.vector.y != 0))
146  {
147  angle = atan2(target_vector_transformed.vector.y, target_vector_transformed.vector.x);
148  angle -= atan2(source_vector_transformed.vector.y, source_vector_transformed.vector.x);
149  }
150 
151  // Rate limit the rotation.
152  if (config_.max_angular_rate == 0)
153  angle_ = angle;
154  else
155  {
156  double delta = fmod(angle - angle_, 2.0 * M_PI);
157  if (delta > M_PI)
158  delta -= 2.0 * M_PI;
159  else if (delta < - M_PI)
160  delta += 2.0 * M_PI;
161 
162  double max_delta = config_.max_angular_rate * (msg->header.stamp - prev_stamp_).toSec();
163  if (delta > max_delta)
164  delta = max_delta;
165  else if (delta < -max_delta)
166  delta = - max_delta;
167 
168  angle_ += delta;
169  }
170  angle_ = fmod(angle_, 2.0 * M_PI);
171  }
172  catch (tf2::TransformException &e)
173  {
174  NODELET_ERROR("Transform error: %s", e.what());
175  }
176 
177  //NODELET_INFO("angle: %f", 180 * angle_ / M_PI);
178 
179  // Publish the transform.
180  geometry_msgs::TransformStamped transform;
181  transform.transform.translation.x = 0;
182  transform.transform.translation.y = 0;
183  transform.transform.translation.z = 0;
184  tf2::convert(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), angle_), transform.transform.rotation);
185  transform.header.frame_id = msg->header.frame_id;
186  transform.child_frame_id = frameWithDefault(config_.output_frame_id, msg->header.frame_id + "_rotated");
187  transform.header.stamp = msg->header.stamp;
188  tf_pub_.sendTransform(transform);
189 
190  // Transform the image.
191  try
192  {
193  // Convert the image into something opencv can handle.
194  cv::Mat in_image = cv_bridge::toCvShare(msg, msg->encoding)->image;
195 
196  // Compute the output image size.
197  int max_dim = in_image.cols > in_image.rows ? in_image.cols : in_image.rows;
198  int min_dim = in_image.cols < in_image.rows ? in_image.cols : in_image.rows;
199  int noblack_dim = min_dim / sqrt(2);
200  int diag_dim = sqrt(in_image.cols*in_image.cols + in_image.rows*in_image.rows);
201  int out_size;
202  int candidates[] = { noblack_dim, min_dim, max_dim, diag_dim, diag_dim }; // diag_dim repeated to simplify limit case.
203  int step = config_.output_image_size;
204  out_size = candidates[step] + (candidates[step + 1] - candidates[step]) * (config_.output_image_size - step);
205  //NODELET_INFO("out_size: %d", out_size);
206 
207  // Compute the rotation matrix.
208  cv::Mat rot_matrix = cv::getRotationMatrix2D(cv::Point2f(in_image.cols / 2.0, in_image.rows / 2.0), 180 * angle_ / M_PI, 1);
209  cv::Mat translation = rot_matrix.col(2);
210  rot_matrix.at<double>(0, 2) += (out_size - in_image.cols) / 2.0;
211  rot_matrix.at<double>(1, 2) += (out_size - in_image.rows) / 2.0;
212 
213  // Do the rotation
214  cv::Mat out_image;
215  cv::warpAffine(in_image, out_image, rot_matrix, cv::Size(out_size, out_size));
216 
217  // Publish the image.
218  sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, out_image).toImageMsg();
219  out_img->header.frame_id = transform.child_frame_id;
220  img_pub_.publish(out_img);
221  }
222  catch (cv::Exception &e)
223  {
224  NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
225  }
226 
227  prev_stamp_ = msg->header.stamp;
228  }
229 
230  void subscribe()
231  {
232  NODELET_DEBUG("Subscribing to image topic.");
233  if (config_.use_camera_info && config_.input_frame_id.empty())
234  cam_sub_ = it_->subscribeCamera("image", 3, &ImageRotateNodelet::imageCallbackWithInfo, this);
235  else
236  img_sub_ = it_->subscribe("image", 3, &ImageRotateNodelet::imageCallback, this);
237  }
238 
239  void unsubscribe()
240  {
241  NODELET_DEBUG("Unsubscribing from image topic.");
242  img_sub_.shutdown();
243  cam_sub_.shutdown();
244  }
245 
247  {
248  if (subscriber_count_++ == 0) {
249  subscribe();
250  }
251  }
252 
254  {
255  subscriber_count_--;
256  if (subscriber_count_ == 0) {
257  unsubscribe();
258  }
259  }
260 
261 public:
262  virtual void onInit()
263  {
264  nh_ = getNodeHandle();
266  subscriber_count_ = 0;
267  angle_ = 0;
268  prev_stamp_ = ros::Time(0, 0);
269  tf_sub_.reset(new tf2_ros::TransformListener(tf_buffer_));
271  image_transport::SubscriberStatusCallback disconnect_cb = boost::bind(&ImageRotateNodelet::disconnectCb, this, _1);
272  img_pub_ = image_transport::ImageTransport(ros::NodeHandle(nh_, "rotated")).advertise("image", 1, connect_cb, disconnect_cb);
273 
274  dynamic_reconfigure::Server<image_rotate::ImageRotateConfig>::CallbackType f =
275  boost::bind(&ImageRotateNodelet::reconfigureCallback, this, _1, _2);
276  srv.setCallback(f);
277  }
278 };
279 }
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
const std::string & frameWithDefault(const std::string &frame, const std::string &image_frame)
#define NODELET_ERROR(...)
TF2SIMD_FORCE_INLINE tf2Scalar angle(const Quaternion &q1, const Quaternion &q2)
f
void connectCb(const image_transport::SingleSubscriberPublisher &ssp)
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
void disconnectCb(const image_transport::SingleSubscriberPublisher &)
PLUGINLIB_EXPORT_CLASS(image_rotate::ImageRotateNodelet, nodelet::Nodelet)
void do_work(const sensor_msgs::ImageConstPtr &msg, const std::string input_frame_from_msg)
void reconfigureCallback(image_rotate::ImageRotateConfig &new_config, uint32_t level)
image_transport::CameraSubscriber cam_sub_
boost::shared_ptr< tf2_ros::TransformListener > tf_sub_
void publish(const sensor_msgs::Image &message) const
tf2_ros::TransformBroadcaster tf_pub_
geometry_msgs::Vector3Stamped source_vector_
image_transport::Subscriber img_sub_
boost::shared_ptr< image_transport::ImageTransport > it_
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
ros::NodeHandle & getNodeHandle() const
geometry_msgs::Vector3Stamped target_vector_
void sendTransform(const geometry_msgs::TransformStamped &transform)
unsigned int step
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
void convert(const A &a, B &b)
image_rotate::ImageRotateConfig config_
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
image_transport::Publisher img_pub_
void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &cam_info)
dynamic_reconfigure::Server< image_rotate::ImageRotateConfig > srv
#define NODELET_DEBUG(...)
sensor_msgs::ImagePtr toImageMsg() const


image_rotate
Author(s): Blaise Gassend
autogenerated on Fri May 3 2019 02:59:55