52 #include <jsk_pcl_ros/ImageRotateConfig.h> 53 #include <geometry_msgs/Vector3Stamped.h> 56 #include <opencv2/imgproc/imgproc.hpp> 57 #include <dynamic_reconfigure/server.h> 60 #if ROS_VERSION_MINIMUM(1, 10, 0) 75 dynamic_reconfigure::Server<jsk_pcl_ros::ImageRotateConfig>
srv;
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_)
116 if (use_tf2_ != config_.use_tf2) {
117 use_tf2_ = config_.use_tf2;
122 const std::string &
frameWithDefault(
const std::string &frame,
const std::string &image_frame)
131 do_work(msg, cam_info->header.frame_id);
136 do_work(msg, msg->header.frame_id);
140 const std::string& source_frame_id,
const ros::Time& time,
141 const std::string& fixed_frame_id,
147 geometry_msgs::TransformStamped trans
148 = tf2_client_->lookupTransform(input_frame_id, source_frame_id,
149 target_time, duration);
151 Eigen::Affine3d transform_eigen;
160 target_vector.
frame_id_ = input_frame_id;
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);
173 void do_work(
const sensor_msgs::ImageConstPtr&
msg,
const std::string input_frame_from_msg)
177 std::string input_frame_id =
frameWithDefault(config_.input_frame_id, input_frame_from_msg);
180 target_vector_.
stamp_ = msg->header.stamp;
185 input_frame_id, target_vector_, target_vector_transformed,
189 source_vector_.
stamp_ = msg->header.stamp;
194 input_frame_id, source_vector_, source_vector_transformed,
204 if ((target_vector_transformed.x() != 0 || target_vector_transformed.y() != 0) &&
205 (source_vector_transformed.x() != 0 || source_vector_transformed.y() != 0))
207 angle =
atan2(target_vector_transformed.y(), target_vector_transformed.x());
208 angle -=
atan2(source_vector_transformed.y(), source_vector_transformed.x());
212 if (config_.max_angular_rate == 0)
216 double delta = fmod(angle - angle_, 2.0 *
M_PI);
219 else if (delta < -
M_PI)
222 double max_delta = config_.max_angular_rate * (msg->header.stamp -
prev_stamp_).toSec();
223 if (delta > max_delta)
225 else if (delta < -max_delta)
230 angle_ = fmod(angle_, 2.0 *
M_PI);
234 NODELET_ERROR(
"[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
248 transform.
frame_id_ = msg->header.frame_id;
250 transform.
stamp_ = msg->header.stamp;
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);
265 int candidates[] = { noblack_dim, min_dim, max_dim, diag_dim, diag_dim };
266 int step = config_.output_image_size;
267 out_size = candidates[step] + (candidates[step + 1] - candidates[step]) * (config_.output_image_size - step);
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;
278 cv::warpAffine(in_image, out_image, rot_matrix, cv::Size(out_size, out_size));
285 catch (cv::Exception &e)
287 NODELET_ERROR(
"Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
291 prev_stamp_ = msg->header.stamp;
297 if (config_.use_camera_info && config_.input_frame_id.empty())
312 if (subscriber_count_++ == 0) {
320 if (subscriber_count_ == 0) {
331 subscriber_count_ = 0;
338 dynamic_reconfigure::Server<jsk_pcl_ros::ImageRotateConfig>::CallbackType
f =
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t)
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)
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_
void disconnectCb(const image_transport::SingleSubscriberPublisher &)
void output(int index, double value)
void publish(const sensor_msgs::Image &message) const
boost::shared_ptr< image_transport::ImageTransport > it_
dynamic_reconfigure::Server< jsk_pcl_ros::ImageRotateConfig > srv
ros::NodeHandle & getNodeHandle() const
void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &cam_info)
tf::Stamped< tf::Vector3 > source_vector_
tf::Stamped< tf::Vector3 > target_vector_
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
image_transport::Subscriber img_sub_
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::ImageRotateNodelet, nodelet::Nodelet)
const std::string & frameWithDefault(const std::string &frame, const std::string &image_frame)
tf::TransformBroadcaster tf_pub_
#define NODELET_DEBUG(...)
boost::shared_ptr< tf::TransformListener > tf_sub_
sensor_msgs::ImagePtr toImageMsg() const
image_transport::Publisher img_pub_
void imageCallback(const sensor_msgs::ImageConstPtr &msg)