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, Willow Garage, 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 Willow Garage 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 <tf2_ros/transform_listener.h>
00047 #include <tf2_ros/transform_broadcaster.h>
00048 #include <image_rotate/ImageRotateConfig.h>
00049 #include <geometry_msgs/Vector3Stamped.h>
00050 #include <image_transport/image_transport.h>
00051 #include <cv_bridge/cv_bridge.h>
00052 #include <opencv2/imgproc/imgproc.hpp>
00053 #include <dynamic_reconfigure/server.h>
00054 #include <math.h>
00055 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00056 
00057 namespace image_rotate {
00058 class ImageRotateNodelet : public nodelet::Nodelet
00059 {
00060   tf2_ros::Buffer tf_buffer_;
00061   boost::shared_ptr<tf2_ros::TransformListener> tf_sub_;
00062   tf2_ros::TransformBroadcaster tf_pub_;
00063 
00064   image_rotate::ImageRotateConfig config_;
00065   dynamic_reconfigure::Server<image_rotate::ImageRotateConfig> srv;
00066 
00067   image_transport::Publisher img_pub_;
00068   image_transport::Subscriber img_sub_;
00069   image_transport::CameraSubscriber cam_sub_;
00070 
00071   geometry_msgs::Vector3Stamped target_vector_;
00072   geometry_msgs::Vector3Stamped source_vector_;
00073 
00074   boost::shared_ptr<image_transport::ImageTransport> it_;
00075   ros::NodeHandle nh_;
00076 
00077   int subscriber_count_;
00078   double angle_;
00079   ros::Time prev_stamp_;
00080 
00081   void reconfigureCallback(image_rotate::ImageRotateConfig &new_config, uint32_t level)
00082   {
00083     config_ = new_config;
00084     target_vector_.vector.x = config_.target_x;
00085     target_vector_.vector.y = config_.target_y;
00086     target_vector_.vector.z = config_.target_z;
00087 
00088     source_vector_.vector.x = config_.source_x;
00089     source_vector_.vector.y = config_.source_y;
00090     source_vector_.vector.z = config_.source_z;
00091     if (subscriber_count_)
00092     { // @todo Could do this without an interruption at some point.
00093       unsubscribe();
00094       subscribe();
00095     }
00096   }
00097 
00098   const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
00099   {
00100     if (frame.empty())
00101       return image_frame;
00102     return frame;
00103   }
00104 
00105   void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
00106   {
00107     do_work(msg, cam_info->header.frame_id);
00108   }
00109   
00110   void imageCallback(const sensor_msgs::ImageConstPtr& msg)
00111   {
00112     do_work(msg, msg->header.frame_id);
00113   }
00114 
00115   void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
00116   {
00117     try
00118     {
00119       std::string input_frame_id = frameWithDefault(config_.input_frame_id, input_frame_from_msg);
00120       std::string target_frame_id = frameWithDefault(config_.target_frame_id, input_frame_from_msg);
00121       std::string source_frame_id = frameWithDefault(config_.source_frame_id, input_frame_from_msg);
00122 
00123       // Transform the target vector into the image frame.
00124       target_vector_.header.stamp = msg->header.stamp;
00125       target_vector_.header.frame_id = target_frame_id;
00126       geometry_msgs::Vector3Stamped target_vector_transformed;
00127       geometry_msgs::TransformStamped transform = tf_buffer_.lookupTransform(target_frame_id, input_frame_id, msg->header.stamp, msg->header.stamp-prev_stamp_);
00128       tf2::doTransform(target_vector_, target_vector_transformed, transform);
00129 
00130       // Transform the source vector into the image frame.
00131       source_vector_.header.stamp = msg->header.stamp;
00132       source_vector_.header.frame_id = source_frame_id;
00133       geometry_msgs::Vector3Stamped source_vector_transformed;
00134       transform = tf_buffer_.lookupTransform(source_frame_id, input_frame_id, msg->header.stamp, msg->header.stamp-prev_stamp_);
00135       tf2::doTransform(source_vector_, source_vector_transformed, transform);
00136 
00137       //NODELET_INFO("target: %f %f %f", target_vector_.x(), target_vector_.y(), target_vector_.z());
00138       //NODELET_INFO("target_transformed: %f %f %f", target_vector_transformed.x(), target_vector_transformed.y(), target_vector_transformed.z());
00139       //NODELET_INFO("source: %f %f %f", source_vector_.x(), source_vector_.y(), source_vector_.z());
00140       //NODELET_INFO("source_transformed: %f %f %f", source_vector_transformed.x(), source_vector_transformed.y(), source_vector_transformed.z());
00141 
00142       // Calculate the angle of the rotation.
00143       double angle = angle_;
00144       if ((target_vector_transformed.vector.x    != 0 || target_vector_transformed.vector.y    != 0) &&
00145           (source_vector_transformed.vector.x != 0 || source_vector_transformed.vector.y != 0))
00146       {
00147         angle = atan2(target_vector_transformed.vector.y, target_vector_transformed.vector.x);
00148         angle -= atan2(source_vector_transformed.vector.y, source_vector_transformed.vector.x);
00149       }
00150 
00151       // Rate limit the rotation.
00152       if (config_.max_angular_rate == 0)
00153         angle_ = angle;
00154       else
00155       {
00156         double delta = fmod(angle - angle_, 2.0 * M_PI);
00157         if (delta > M_PI)
00158           delta -= 2.0 * M_PI;
00159         else if (delta < - M_PI)
00160           delta += 2.0 * M_PI;
00161 
00162         double max_delta = config_.max_angular_rate * (msg->header.stamp - prev_stamp_).toSec();
00163         if (delta > max_delta)
00164           delta = max_delta;
00165         else if (delta < -max_delta)
00166           delta = - max_delta;
00167 
00168         angle_ += delta;
00169       }
00170       angle_ = fmod(angle_, 2.0 * M_PI);
00171     }
00172     catch (tf2::TransformException &e)
00173     {
00174       NODELET_ERROR("Transform error: %s", e.what());
00175     }
00176 
00177     //NODELET_INFO("angle: %f", 180 * angle_ / M_PI);
00178 
00179     // Publish the transform.
00180     geometry_msgs::TransformStamped transform;
00181     transform.transform.translation.x = 0;
00182     transform.transform.translation.y = 0;
00183     transform.transform.translation.z = 0;
00184     tf2::convert(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), angle_), transform.transform.rotation);
00185     transform.header.frame_id = msg->header.frame_id;
00186     transform.child_frame_id = frameWithDefault(config_.output_frame_id, msg->header.frame_id + "_rotated");
00187     transform.header.stamp = msg->header.stamp;
00188     tf_pub_.sendTransform(transform);
00189 
00190     // Transform the image.
00191     try
00192     {
00193       // Convert the image into something opencv can handle.
00194       cv::Mat in_image = cv_bridge::toCvShare(msg, msg->encoding)->image;
00195 
00196       // Compute the output image size.
00197       int max_dim = in_image.cols > in_image.rows ? in_image.cols : in_image.rows;
00198       int min_dim = in_image.cols < in_image.rows ? in_image.cols : in_image.rows;
00199       int noblack_dim = min_dim / sqrt(2);
00200       int diag_dim = sqrt(in_image.cols*in_image.cols + in_image.rows*in_image.rows);
00201       int out_size;
00202       int candidates[] = { noblack_dim, min_dim, max_dim, diag_dim, diag_dim }; // diag_dim repeated to simplify limit case.
00203       int step = config_.output_image_size;
00204       out_size = candidates[step] + (candidates[step + 1] - candidates[step]) * (config_.output_image_size - step);
00205       //NODELET_INFO("out_size: %d", out_size);
00206 
00207       // Compute the rotation matrix.
00208       cv::Mat rot_matrix = cv::getRotationMatrix2D(cv::Point2f(in_image.cols / 2.0, in_image.rows / 2.0), 180 * angle_ / M_PI, 1);
00209       cv::Mat translation = rot_matrix.col(2);
00210       rot_matrix.at<double>(0, 2) += (out_size - in_image.cols) / 2.0;
00211       rot_matrix.at<double>(1, 2) += (out_size - in_image.rows) / 2.0;
00212 
00213       // Do the rotation
00214       cv::Mat out_image;
00215       cv::warpAffine(in_image, out_image, rot_matrix, cv::Size(out_size, out_size));
00216 
00217       // Publish the image.
00218       sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, out_image).toImageMsg();
00219       out_img->header.frame_id = transform.child_frame_id;
00220       img_pub_.publish(out_img);
00221     }
00222     catch (cv::Exception &e)
00223     {
00224       NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
00225     }
00226 
00227     prev_stamp_ = msg->header.stamp;
00228   }
00229 
00230   void subscribe()
00231   {
00232     NODELET_DEBUG("Subscribing to image topic.");
00233     if (config_.use_camera_info && config_.input_frame_id.empty())
00234       cam_sub_ = it_->subscribeCamera("image", 3, &ImageRotateNodelet::imageCallbackWithInfo, this);
00235     else
00236       img_sub_ = it_->subscribe("image", 3, &ImageRotateNodelet::imageCallback, this);
00237   }
00238 
00239   void unsubscribe()
00240   {
00241       NODELET_DEBUG("Unsubscribing from image topic.");
00242       img_sub_.shutdown();
00243       cam_sub_.shutdown();
00244   }
00245 
00246   void connectCb(const image_transport::SingleSubscriberPublisher& ssp)
00247   {
00248     if (subscriber_count_++ == 0) {
00249       subscribe();
00250     }
00251   }
00252 
00253   void disconnectCb(const image_transport::SingleSubscriberPublisher&)
00254   {
00255     subscriber_count_--;
00256     if (subscriber_count_ == 0) {
00257       unsubscribe();
00258     }
00259   }
00260 
00261 public:
00262   virtual void onInit()
00263   {
00264     nh_ = getNodeHandle();
00265     it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
00266     subscriber_count_ = 0;
00267     angle_ = 0;
00268     prev_stamp_ = ros::Time(0, 0);
00269     tf_sub_.reset(new tf2_ros::TransformListener(tf_buffer_));
00270     image_transport::SubscriberStatusCallback connect_cb    = boost::bind(&ImageRotateNodelet::connectCb, this, _1);
00271     image_transport::SubscriberStatusCallback disconnect_cb = boost::bind(&ImageRotateNodelet::disconnectCb, this, _1);
00272     img_pub_ = image_transport::ImageTransport(ros::NodeHandle(nh_, "rotated")).advertise("image", 1, connect_cb, disconnect_cb);
00273 
00274     dynamic_reconfigure::Server<image_rotate::ImageRotateConfig>::CallbackType f =
00275       boost::bind(&ImageRotateNodelet::reconfigureCallback, this, _1, _2);
00276     srv.setCallback(f);
00277   }
00278 };
00279 }
00280 #include <pluginlib/class_list_macros.h>
00281 PLUGINLIB_EXPORT_CLASS(image_rotate::ImageRotateNodelet, nodelet::Nodelet);


image_rotate
Author(s): Blaise Gassend
autogenerated on Wed May 1 2019 02:51:50