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, JSK Lab, 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 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 /********************************************************************
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>
47 #include <tf/transform_listener.h>
49 #include <tf2_ros/buffer_client.h>
52 #include <jsk_pcl_ros/ImageRotateConfig.h>
53 #include <geometry_msgs/Vector3Stamped.h>
55 #include <cv_bridge/cv_bridge.h>
56 #include <opencv2/imgproc/imgproc.hpp>
57 #include <dynamic_reconfigure/server.h>
58 #include <math.h>
59 
60 #if ROS_VERSION_MINIMUM(1, 10, 0)
61 // hydro and later
62 #else
63 // groovy
64 #define tf2_ros tf2
65 #endif
66 
67 namespace jsk_pcl_ros {
69 {
70  bool use_tf2_;
74  jsk_pcl_ros::ImageRotateConfig config_;
75  dynamic_reconfigure::Server<jsk_pcl_ros::ImageRotateConfig> srv;
76 
80 
83 
86 
88  double angle_;
90 
92  {
93  if (use_tf2_) {
94  // shutdown tf_sub_
95  if (tf_sub_) {
96  tf_sub_.reset();
97  }
98  }
99  else {
100  if(!tf_sub_) {
101  tf_sub_.reset(new tf::TransformListener());
102  }
103  }
104  }
105 
106  void reconfigureCallback(jsk_pcl_ros::ImageRotateConfig &new_config, uint32_t level)
107  {
108  config_ = new_config;
109  target_vector_.setValue(config_.target_x, config_.target_y, config_.target_z);
110  source_vector_.setValue(config_.source_x, config_.source_y, config_.source_z);
111  if (subscriber_count_)
112  { // @todo Could do this without an interruption at some point.
113  unsubscribe();
114  subscribe();
115  }
116  if (use_tf2_ != config_.use_tf2) {
117  use_tf2_ = config_.use_tf2;
118  setupTFListener();
119  }
120  }
121 
122  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
123  {
124  if (frame.empty())
125  return image_frame;
126  return frame;
127  }
128 
129  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
130  {
131  do_work(msg, cam_info->header.frame_id);
132  }
133 
134  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
135  {
136  do_work(msg, msg->header.frame_id);
137  }
138 
139  void transformVector(const std::string& input_frame_id, const ros::Time& target_time,
140  const std::string& source_frame_id, const ros::Time& time,
141  const std::string& fixed_frame_id,
142  const tf::Stamped<tf::Vector3>& input_vector,
143  tf::Stamped<tf::Vector3>& target_vector,
144  const ros::Duration& duration)
145  {
146  if (use_tf2_) {
147  geometry_msgs::TransformStamped trans
148  = tf2_client_->lookupTransform(input_frame_id, source_frame_id,
149  target_time, duration);
150  // geometry_msgs -> eigen -> tf
151  Eigen::Affine3d transform_eigen;
152  tf::transformMsgToEigen(trans.transform, transform_eigen);
153  tf::StampedTransform transform_tf; // convert trans to tfStampedTransform
154  tf::transformEigenToTF(transform_eigen, transform_tf);
155  tf::Vector3 origin = tf::Vector3(0, 0, 0);
156  tf::Vector3 end = input_vector;
157  tf::Vector3 output = (transform_tf * end) - (transform_tf * origin);
158  target_vector.setData(output);
159  target_vector.stamp_ = input_vector.stamp_;
160  target_vector.frame_id_ = input_frame_id;
161  }
162  else {
163  if(tf_sub_){
164  tf_sub_->waitForTransform(input_frame_id, target_time,
165  source_frame_id, time,
166  fixed_frame_id, duration);
167  tf_sub_->transformVector(input_frame_id, target_time, input_vector,
168  fixed_frame_id, target_vector);
169  }
170  }
171  }
172 
173  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
174  {
175  try
176  {
177  std::string input_frame_id = frameWithDefault(config_.input_frame_id, input_frame_from_msg);
178 
179  // Transform the target vector into the image frame.
180  target_vector_.stamp_ = msg->header.stamp;
181  target_vector_.frame_id_ = frameWithDefault(config_.target_frame_id, input_frame_id);
182  tf::Stamped<tf::Vector3> target_vector_transformed;
183  transformVector(input_frame_id, msg->header.stamp,
184  target_vector_.frame_id_, target_vector_.stamp_,
185  input_frame_id, target_vector_, target_vector_transformed,
186  ros::Duration(0.2));
187 
188  // Transform the source vector into the image frame.
189  source_vector_.stamp_ = msg->header.stamp;
190  source_vector_.frame_id_ = frameWithDefault(config_.source_frame_id, input_frame_id);
191  tf::Stamped<tf::Vector3> source_vector_transformed;
192  transformVector(input_frame_id, msg->header.stamp,
193  source_vector_.frame_id_, source_vector_.stamp_,
194  input_frame_id, source_vector_, source_vector_transformed,
195  ros::Duration(0.01));
196 
197  // NODELET_INFO("target: %f %f %f", target_vector_.x(), target_vector_.y(), target_vector_.z());
198  // NODELET_INFO("target_transformed: %f %f %f", target_vector_transformed.x(), target_vector_transformed.y(), target_vector_transformed.z());
199  // NODELET_INFO("source: %f %f %f", source_vector_.x(), source_vector_.y(), source_vector_.z());
200  // NODELET_INFO("source_transformed: %f %f %f", source_vector_transformed.x(), source_vector_transformed.y(), source_vector_transformed.z());
201 
202  // Calculate the angle of the rotation.
203  double angle = angle_;
204  if ((target_vector_transformed.x() != 0 || target_vector_transformed.y() != 0) &&
205  (source_vector_transformed.x() != 0 || source_vector_transformed.y() != 0))
206  {
207  angle = atan2(target_vector_transformed.y(), target_vector_transformed.x());
208  angle -= atan2(source_vector_transformed.y(), source_vector_transformed.x());
209  }
210 
211  // Rate limit the rotation.
212  if (config_.max_angular_rate == 0)
213  angle_ = angle;
214  else
215  {
216  double delta = fmod(angle - angle_, 2.0 * M_PI);
217  if (delta > M_PI)
218  delta -= 2.0 * M_PI;
219  else if (delta < - M_PI)
220  delta += 2.0 * M_PI;
221 
222  double max_delta = config_.max_angular_rate * (msg->header.stamp - prev_stamp_).toSec();
223  if (delta > max_delta)
224  delta = max_delta;
225  else if (delta < -max_delta)
226  delta = - max_delta;
227 
228  angle_ += delta;
229  }
230  angle_ = fmod(angle_, 2.0 * M_PI);
231  }
232  catch (tf2::TransformException &e)
233  {
234  NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
235  }
236  catch (...)
237  {
238  NODELET_ERROR("[%s] Transform error", __PRETTY_FUNCTION__);
239  }
240 
241 
242  //NODELET_INFO("angle: %f", 180 * angle_ / M_PI);
243 
244  // Publish the transform.
245  tf::StampedTransform transform;
246  transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
247  transform.setRotation(tf::Quaternion(tf::Vector3(0.0, 0.0, 1.0), angle_));
248  transform.frame_id_ = msg->header.frame_id;
249  transform.child_frame_id_ = frameWithDefault(config_.output_frame_id, msg->header.frame_id + "_rotated");
250  transform.stamp_ = msg->header.stamp;
251  tf_pub_.sendTransform(transform);
252 
253  // Transform the image.
254  try
255  {
256  // Convert the image into something opencv can handle.
257  cv::Mat in_image = cv_bridge::toCvShare(msg, msg->encoding)->image;
258 
259  // Compute the output image size.
260  int max_dim = in_image.cols > in_image.rows ? in_image.cols : in_image.rows;
261  int min_dim = in_image.cols < in_image.rows ? in_image.cols : in_image.rows;
262  int noblack_dim = min_dim / sqrt(2);
263  int diag_dim = sqrt(in_image.cols*in_image.cols + in_image.rows*in_image.rows);
264  int out_size;
265  int candidates[] = { noblack_dim, min_dim, max_dim, diag_dim, diag_dim }; // diag_dim repeated to simplify limit case.
266  int step = config_.output_image_size;
267  out_size = candidates[step] + (candidates[step + 1] - candidates[step]) * (config_.output_image_size - step);
268  //NODELET_INFO("out_size: %d", out_size);
269 
270  // Compute the rotation matrix.
271  cv::Mat rot_matrix = cv::getRotationMatrix2D(cv::Point2f(in_image.cols / 2.0, in_image.rows / 2.0), 180 * angle_ / M_PI, 1);
272  cv::Mat translation = rot_matrix.col(2);
273  rot_matrix.at<double>(0, 2) += (out_size - in_image.cols) / 2.0;
274  rot_matrix.at<double>(1, 2) += (out_size - in_image.rows) / 2.0;
275 
276  // Do the rotation
277  cv::Mat out_image;
278  cv::warpAffine(in_image, out_image, rot_matrix, cv::Size(out_size, out_size));
279 
280  // Publish the image.
281  sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, out_image).toImageMsg();
282  out_img->header.frame_id = transform.child_frame_id_;
283  img_pub_.publish(out_img);
284  }
285  catch (cv::Exception &e)
286  {
287  NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
288  }
289 
290 
291  prev_stamp_ = msg->header.stamp;
292  }
293 
294  void subscribe()
295  {
296  NODELET_DEBUG("Subscribing to image topic.");
297  if (config_.use_camera_info && config_.input_frame_id.empty())
298  cam_sub_ = it_->subscribeCamera("image", 3, &ImageRotateNodelet::imageCallbackWithInfo, this);
299  else
300  img_sub_ = it_->subscribe("image", 3, &ImageRotateNodelet::imageCallback, this);
301  }
302 
303  void unsubscribe()
304  {
305  NODELET_DEBUG("Unsubscribing from image topic.");
306  img_sub_.shutdown();
307  cam_sub_.shutdown();
308  }
309 
311  {
312  if (subscriber_count_++ == 0) {
313  subscribe();
314  }
315  }
316 
318  {
319  subscriber_count_--;
320  if (subscriber_count_ == 0) {
321  unsubscribe();
322  }
323  }
324 
325 public:
326  virtual void onInit()
327  {
328  nh_ = getNodeHandle();
330  tf2_client_.reset(new tf2_ros::BufferClient("tf2_buffer_server", 100, ros::Duration(0.2)));
331  subscriber_count_ = 0;
332  angle_ = 0;
333  prev_stamp_ = ros::Time(0, 0);
335  image_transport::SubscriberStatusCallback disconnect_cb = boost::bind(&ImageRotateNodelet::disconnectCb, this, _1);
336  img_pub_ = image_transport::ImageTransport(ros::NodeHandle(nh_, "rotated")).advertise("image", 1, connect_cb, disconnect_cb);
337 
338  dynamic_reconfigure::Server<jsk_pcl_ros::ImageRotateConfig>::CallbackType f =
339  boost::bind(&ImageRotateNodelet::reconfigureCallback, this, _1, _2);
340  srv.setCallback(f);
341  }
342 };
343 }
344 
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t)
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
jsk_pcl_ros::ImageRotateConfig config_
#define NODELET_ERROR(...)
void setData(const T &input)
void transformVector(const std::string &input_frame_id, const ros::Time &target_time, const std::string &source_frame_id, const ros::Time &time, const std::string &fixed_frame_id, const tf::Stamped< tf::Vector3 > &input_vector, tf::Stamped< tf::Vector3 > &target_vector, const ros::Duration &duration)
end
image_transport::CameraSubscriber cam_sub_
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
void connectCb(const image_transport::SingleSubscriberPublisher &ssp)
void reconfigureCallback(jsk_pcl_ros::ImageRotateConfig &new_config, uint32_t level)
void do_work(const sensor_msgs::ImageConstPtr &msg, const std::string input_frame_from_msg)
boost::shared_ptr< tf2_ros::BufferClient > tf2_client_
std::string child_frame_id_
void disconnectCb(const image_transport::SingleSubscriberPublisher &)
void output(int index, double value)
double atan2()
GLfloat angle
#define M_PI
void publish(const sensor_msgs::Image &message) const
double sqrt()
boost::shared_ptr< image_transport::ImageTransport > it_
dynamic_reconfigure::Server< jsk_pcl_ros::ImageRotateConfig > srv
void sendTransform(const StampedTransform &transform)
ros::NodeHandle & getNodeHandle() const
void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &cam_info)
tf::Stamped< tf::Vector3 > source_vector_
ros::Time stamp_
tf::Stamped< tf::Vector3 > target_vector_
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
std::string frame_id_
image_transport::Subscriber img_sub_
step
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::ImageRotateNodelet, nodelet::Nodelet)
const std::string & frameWithDefault(const std::string &frame, const std::string &image_frame)
#define NODELET_DEBUG(...)
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
boost::shared_ptr< tf::TransformListener > tf_sub_
sensor_msgs::ImagePtr toImageMsg() const
std::string frame_id_
image_transport::Publisher img_pub_
void imageCallback(const sensor_msgs::ImageConstPtr &msg)


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