image_rotate_nodelet.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2014, JSK Lab.
00005 *                2008, JSK Lab, Inc.
00006 *  All rights reserved.
00007 * 
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 * 
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the JSK Lab nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 * 
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *********************************************************************/
00035 
00036 /********************************************************************
00037 * image_rotate_nodelet.cpp
00038 * this is a forked version of image_rotate.
00039 * this image_rotate_nodelet supports:
00040 *  1) nodelet
00041 *  2) tf and tf2
00042 *********************************************************************/
00043 
00044 #include <ros/ros.h>
00045 #include <nodelet/nodelet.h>
00046 #include <jsk_topic_tools/log_utils.h>
00047 #include <tf/transform_listener.h>
00048 #include <tf/transform_broadcaster.h>
00049 #include <tf2_ros/buffer_client.h>
00050 #include <eigen_conversions/eigen_msg.h>
00051 #include <tf_conversions/tf_eigen.h>
00052 #include <jsk_pcl_ros/ImageRotateConfig.h>
00053 #include <geometry_msgs/Vector3Stamped.h>
00054 #include <image_transport/image_transport.h>
00055 #include <cv_bridge/cv_bridge.h>
00056 #include <opencv2/imgproc/imgproc.hpp>
00057 #include <dynamic_reconfigure/server.h>
00058 #include <math.h>
00059 
00060 #if ROS_VERSION_MINIMUM(1, 10, 0)
00061 // hydro and later
00062 #else
00063 // groovy
00064 #define tf2_ros tf2
00065 #endif
00066 
00067 namespace jsk_pcl_ros {
00068 class ImageRotateNodelet : public nodelet::Nodelet
00069 {
00070   bool use_tf2_;
00071   boost::shared_ptr<tf::TransformListener> tf_sub_;
00072   tf::TransformBroadcaster tf_pub_;
00073   boost::shared_ptr<tf2_ros::BufferClient> tf2_client_;
00074   jsk_pcl_ros::ImageRotateConfig config_;
00075   dynamic_reconfigure::Server<jsk_pcl_ros::ImageRotateConfig> srv;
00076 
00077   image_transport::Publisher img_pub_;
00078   image_transport::Subscriber img_sub_;
00079   image_transport::CameraSubscriber cam_sub_;
00080 
00081   tf::Stamped<tf::Vector3> target_vector_;
00082   tf::Stamped<tf::Vector3> source_vector_;
00083     
00084   boost::shared_ptr<image_transport::ImageTransport> it_;
00085   ros::NodeHandle nh_;
00086 
00087   int subscriber_count_;
00088   double angle_;
00089   ros::Time prev_stamp_;
00090 
00091   void setupTFListener()
00092   {
00093     if (use_tf2_) {
00094       // shutdown tf_sub_
00095       if (tf_sub_) {
00096         tf_sub_.reset();
00097       }
00098     }
00099     else {
00100       if(!tf_sub_) {
00101         tf_sub_.reset(new tf::TransformListener());
00102       }
00103     }
00104   }
00105   
00106   void reconfigureCallback(jsk_pcl_ros::ImageRotateConfig &new_config, uint32_t level)
00107   {
00108     config_ = new_config;
00109     target_vector_.setValue(config_.target_x, config_.target_y, config_.target_z);
00110     source_vector_.setValue(config_.source_x, config_.source_y, config_.source_z);
00111     if (subscriber_count_)
00112     { // @todo Could do this without an interruption at some point.
00113       unsubscribe();
00114       subscribe();
00115     }
00116     if (use_tf2_ != config_.use_tf2) {
00117       use_tf2_ = config_.use_tf2;
00118       setupTFListener();
00119     }
00120   }
00121 
00122   const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
00123   {
00124     if (frame.empty())
00125       return image_frame;
00126     return frame;
00127   }
00128 
00129   void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
00130   {
00131     do_work(msg, cam_info->header.frame_id);
00132   }
00133   
00134   void imageCallback(const sensor_msgs::ImageConstPtr& msg)
00135   {
00136     do_work(msg, msg->header.frame_id);
00137   }
00138 
00139   void transformVector(const std::string& input_frame_id, const ros::Time& target_time,
00140                        const std::string& source_frame_id, const ros::Time& time,
00141                        const std::string& fixed_frame_id,
00142                        const tf::Stamped<tf::Vector3>& input_vector,
00143                        tf::Stamped<tf::Vector3>& target_vector,
00144                        const ros::Duration& duration)
00145   {
00146     if (use_tf2_) {
00147       geometry_msgs::TransformStamped trans
00148         = tf2_client_->lookupTransform(input_frame_id, source_frame_id,
00149                                        target_time, duration);
00150       // geometry_msgs -> eigen -> tf
00151       Eigen::Affine3d transform_eigen;
00152       tf::transformMsgToEigen(trans.transform, transform_eigen);
00153       tf::StampedTransform transform_tf; // convert trans to tfStampedTransform
00154       tf::transformEigenToTF(transform_eigen, transform_tf);
00155       tf::Vector3 origin = tf::Vector3(0, 0, 0);
00156       tf::Vector3 end = input_vector;
00157       tf::Vector3 output = (transform_tf * end) - (transform_tf * origin);
00158       target_vector.setData(output);
00159       target_vector.stamp_ = input_vector.stamp_;
00160       target_vector.frame_id_ = input_frame_id;
00161     }
00162     else {
00163       if(tf_sub_){
00164         tf_sub_->waitForTransform(input_frame_id, target_time,
00165                                   source_frame_id, time,
00166                                   fixed_frame_id, duration);
00167         tf_sub_->transformVector(input_frame_id, target_time, input_vector,
00168                                  fixed_frame_id, target_vector);
00169       }
00170     }
00171   }
00172   
00173   void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
00174   {
00175     try
00176     {
00177       std::string input_frame_id = frameWithDefault(config_.input_frame_id, input_frame_from_msg);
00178 
00179       // Transform the target vector into the image frame.
00180       target_vector_.stamp_ = msg->header.stamp;
00181       target_vector_.frame_id_ = frameWithDefault(config_.target_frame_id, input_frame_id);
00182       tf::Stamped<tf::Vector3> target_vector_transformed;
00183       transformVector(input_frame_id, msg->header.stamp,
00184                       target_vector_.frame_id_, target_vector_.stamp_,
00185                       input_frame_id, target_vector_, target_vector_transformed,
00186                       ros::Duration(0.2));
00187 
00188       // Transform the source vector into the image frame.
00189       source_vector_.stamp_ = msg->header.stamp;
00190       source_vector_.frame_id_ = frameWithDefault(config_.source_frame_id, input_frame_id);
00191       tf::Stamped<tf::Vector3> source_vector_transformed;
00192       transformVector(input_frame_id, msg->header.stamp,
00193                       source_vector_.frame_id_, source_vector_.stamp_,
00194                       input_frame_id, source_vector_, source_vector_transformed,
00195                       ros::Duration(0.01));
00196 
00197       // NODELET_INFO("target: %f %f %f", target_vector_.x(), target_vector_.y(), target_vector_.z());
00198       // NODELET_INFO("target_transformed: %f %f %f", target_vector_transformed.x(), target_vector_transformed.y(), target_vector_transformed.z());
00199       // NODELET_INFO("source: %f %f %f", source_vector_.x(), source_vector_.y(), source_vector_.z());
00200       // NODELET_INFO("source_transformed: %f %f %f", source_vector_transformed.x(), source_vector_transformed.y(), source_vector_transformed.z());
00201 
00202       // Calculate the angle of the rotation.
00203       double angle = angle_;
00204       if ((target_vector_transformed.x()    != 0 || target_vector_transformed.y()    != 0) &&
00205           (source_vector_transformed.x() != 0 || source_vector_transformed.y() != 0))
00206       {
00207         angle = atan2(target_vector_transformed.y(), target_vector_transformed.x());
00208         angle -= atan2(source_vector_transformed.y(), source_vector_transformed.x());
00209       }
00210 
00211       // Rate limit the rotation.
00212       if (config_.max_angular_rate == 0)
00213         angle_ = angle;
00214       else
00215       {
00216         double delta = fmod(angle - angle_, 2.0 * M_PI);
00217         if (delta > M_PI)
00218           delta -= 2.0 * M_PI;
00219         else if (delta < - M_PI)
00220           delta += 2.0 * M_PI;
00221 
00222         double max_delta = config_.max_angular_rate * (msg->header.stamp - prev_stamp_).toSec();
00223         if (delta > max_delta)
00224           delta = max_delta;
00225         else if (delta < -max_delta)
00226           delta = - max_delta;
00227 
00228         angle_ += delta;
00229       }
00230       angle_ = fmod(angle_, 2.0 * M_PI);
00231     }
00232     catch (tf2::TransformException &e)
00233     {
00234       NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00235     }
00236     catch (...)
00237     {
00238       NODELET_ERROR("[%s] Transform error", __PRETTY_FUNCTION__);
00239     }
00240 
00241 
00242     //NODELET_INFO("angle: %f", 180 * angle_ / M_PI);
00243 
00244     // Publish the transform.
00245     tf::StampedTransform transform;
00246     transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
00247     transform.setRotation(tf::Quaternion(tf::Vector3(0.0, 0.0, 1.0), angle_));
00248     transform.frame_id_ = msg->header.frame_id;
00249     transform.child_frame_id_ = frameWithDefault(config_.output_frame_id, msg->header.frame_id + "_rotated");
00250     transform.stamp_ = msg->header.stamp;
00251     tf_pub_.sendTransform(transform);
00252 
00253     // Transform the image.
00254     try
00255     {
00256       // Convert the image into something opencv can handle.
00257       cv::Mat in_image = cv_bridge::toCvShare(msg, msg->encoding)->image;
00258 
00259       // Compute the output image size.
00260       int max_dim = in_image.cols > in_image.rows ? in_image.cols : in_image.rows;
00261       int min_dim = in_image.cols < in_image.rows ? in_image.cols : in_image.rows;
00262       int noblack_dim = min_dim / sqrt(2);
00263       int diag_dim = sqrt(in_image.cols*in_image.cols + in_image.rows*in_image.rows);
00264       int out_size;
00265       int candidates[] = { noblack_dim, min_dim, max_dim, diag_dim, diag_dim }; // diag_dim repeated to simplify limit case.
00266       int step = config_.output_image_size;
00267       out_size = candidates[step] + (candidates[step + 1] - candidates[step]) * (config_.output_image_size - step);
00268       //NODELET_INFO("out_size: %d", out_size);
00269 
00270       // Compute the rotation matrix.
00271       cv::Mat rot_matrix = cv::getRotationMatrix2D(cv::Point2f(in_image.cols / 2.0, in_image.rows / 2.0), 180 * angle_ / M_PI, 1);
00272       cv::Mat translation = rot_matrix.col(2);
00273       rot_matrix.at<double>(0, 2) += (out_size - in_image.cols) / 2.0;
00274       rot_matrix.at<double>(1, 2) += (out_size - in_image.rows) / 2.0;
00275 
00276       // Do the rotation
00277       cv::Mat out_image;
00278       cv::warpAffine(in_image, out_image, rot_matrix, cv::Size(out_size, out_size));
00279 
00280       // Publish the image.
00281       sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, out_image).toImageMsg();
00282       out_img->header.frame_id = transform.child_frame_id_;
00283       img_pub_.publish(out_img);
00284     }
00285     catch (cv::Exception &e)
00286     {
00287       NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
00288     }
00289 
00290 
00291     prev_stamp_ = msg->header.stamp;
00292   }
00293 
00294   void subscribe()
00295   {
00296     NODELET_DEBUG("Subscribing to image topic.");
00297     if (config_.use_camera_info && config_.input_frame_id.empty())
00298       cam_sub_ = it_->subscribeCamera("image", 3, &ImageRotateNodelet::imageCallbackWithInfo, this);
00299     else
00300       img_sub_ = it_->subscribe("image", 3, &ImageRotateNodelet::imageCallback, this);
00301   }
00302 
00303   void unsubscribe()
00304   {
00305       NODELET_DEBUG("Unsubscribing from image topic.");
00306       img_sub_.shutdown();
00307       cam_sub_.shutdown();
00308   }
00309 
00310   void connectCb(const image_transport::SingleSubscriberPublisher& ssp)
00311   {
00312     if (subscriber_count_++ == 0) {
00313       subscribe();
00314     }
00315   }
00316 
00317   void disconnectCb(const image_transport::SingleSubscriberPublisher&)
00318   {
00319     subscriber_count_--;
00320     if (subscriber_count_ == 0) {
00321       unsubscribe();
00322     }
00323   }
00324 
00325 public:
00326   virtual void onInit()
00327   {
00328     nh_ = getNodeHandle();
00329     it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
00330     tf2_client_.reset(new tf2_ros::BufferClient("tf2_buffer_server", 100, ros::Duration(0.2)));
00331     subscriber_count_ = 0;
00332     angle_ = 0;
00333     prev_stamp_ = ros::Time(0, 0);
00334     image_transport::SubscriberStatusCallback connect_cb    = boost::bind(&ImageRotateNodelet::connectCb, this, _1);
00335     image_transport::SubscriberStatusCallback disconnect_cb = boost::bind(&ImageRotateNodelet::disconnectCb, this, _1);
00336     img_pub_ = image_transport::ImageTransport(ros::NodeHandle(nh_, "rotated")).advertise("image", 1, connect_cb, disconnect_cb);
00337 
00338     dynamic_reconfigure::Server<jsk_pcl_ros::ImageRotateConfig>::CallbackType f =
00339       boost::bind(&ImageRotateNodelet::reconfigureCallback, this, _1, _2);
00340     srv.setCallback(f);
00341   }
00342 };
00343 }
00344 
00345 #include <pluginlib/class_list_macros.h>
00346 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::ImageRotateNodelet, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:49