fback_flow_nodelet.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, Kei Okada.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Kei Okada nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/fback.cpp
36 /*
37  * This program demonstrates dense optical flow algorithm by Gunnar Farneback
38  * Mainly the function: calcOpticalFlowFarneback()
39  */
40 
41 #include <ros/ros.h>
42 #include "opencv_apps/nodelet.h"
44 #include <cv_bridge/cv_bridge.h>
46 
47 #include <opencv2/highgui/highgui.hpp>
48 #include <opencv2/imgproc/imgproc.hpp>
49 #include <opencv2/video/tracking.hpp>
50 
51 #include <dynamic_reconfigure/server.h>
52 #include "opencv_apps/FBackFlowConfig.h"
53 #include "opencv_apps/FlowArrayStamped.h"
54 
55 namespace opencv_apps
56 {
58 {
63 
65 
66  typedef opencv_apps::FBackFlowConfig Config;
67  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
68  Config config_;
70 
74 
75  std::string window_name_;
76  static bool need_config_update_;
77 
78  cv::Mat prevgray, gray, flow, cflow;
79 
80  void reconfigureCallback(Config& new_config, uint32_t level)
81  {
82  config_ = new_config;
83  }
84 
85  const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame)
86  {
87  if (frame.empty())
88  return image_frame;
89  return frame;
90  }
91 
92  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
93  {
94  doWork(msg, cam_info->header.frame_id);
95  }
96 
97  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
98  {
99  doWork(msg, msg->header.frame_id);
100  }
101 
102  static void trackbarCallback(int /*unused*/, void* /*unused*/)
103  {
104  need_config_update_ = true;
105  }
106 
107  void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg)
108  {
109  // Work on the image.
110  try
111  {
112  // Convert the image into something opencv can handle.
113  cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image;
114 
115  // Messages
116  opencv_apps::FlowArrayStamped flows_msg;
117  flows_msg.header = msg->header;
118 
119  if (debug_view_)
120  {
121  cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
122  if (need_config_update_)
123  {
124  reconfigure_server_->updateConfig(config_);
125  need_config_update_ = false;
126  }
127  }
128 
129  // Check if clock is jumped back
130  if (ros::Time::isSimTime() && prev_stamp_ > msg->header.stamp)
131  {
132  NODELET_WARN_STREAM("Detected jump back in time of " << msg->header.stamp << ". Clearing optical flow cache.");
133  prevgray = cv::Mat();
134  }
135 
136  // Do the work
137  if (frame.channels() > 1)
138  {
139  cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
140  }
141  else
142  {
143  frame.copyTo(gray);
144  }
145  if (prevgray.data)
146  {
147  cv::calcOpticalFlowFarneback(prevgray, gray, flow, 0.5, 3, 15, 3, 5, 1.2, 0);
148  cv::cvtColor(prevgray, cflow, cv::COLOR_GRAY2BGR);
149  // drawOptFlowMap(flow, cflow, 16, 1.5, Scalar(0, 255, 0));
150  int step = 16;
151  cv::Scalar color = cv::Scalar(0, 255, 0);
152  for (int y = 0; y < cflow.rows; y += step)
153  for (int x = 0; x < cflow.cols; x += step)
154  {
155  const cv::Point2f& fxy = flow.at<cv::Point2f>(y, x);
156  cv::line(cflow, cv::Point(x, y), cv::Point(cvRound(x + fxy.x), cvRound(y + fxy.y)), color);
157  cv::circle(cflow, cv::Point(x, y), 2, color, -1);
158 
159  opencv_apps::Flow flow_msg;
160  opencv_apps::Point2D point_msg;
161  opencv_apps::Point2D velocity_msg;
162  point_msg.x = x;
163  point_msg.y = y;
164  velocity_msg.x = fxy.x;
165  velocity_msg.y = fxy.y;
166  flow_msg.point = point_msg;
167  flow_msg.velocity = velocity_msg;
168  flows_msg.flow.push_back(flow_msg);
169  }
170  }
171 
172  std::swap(prevgray, gray);
173 
174  //-- Show what you got
175  if (debug_view_)
176  {
177  cv::imshow(window_name_, cflow);
178  int c = cv::waitKey(1);
179  }
180 
181  // Publish the image.
182  sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", cflow).toImageMsg();
183  img_pub_.publish(out_img);
184  msg_pub_.publish(flows_msg);
185  }
186  catch (cv::Exception& e)
187  {
188  NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
189  }
190 
191  prev_stamp_ = msg->header.stamp;
192  }
193 
194  void subscribe() // NOLINT(modernize-use-override)
195  {
196  NODELET_DEBUG("Subscribing to image topic.");
197  if (config_.use_camera_info)
198  cam_sub_ = it_->subscribeCamera("image", queue_size_, &FBackFlowNodelet::imageCallbackWithInfo, this);
199  else
200  img_sub_ = it_->subscribe("image", queue_size_, &FBackFlowNodelet::imageCallback, this);
201  }
202 
203  void unsubscribe() // NOLINT(modernize-use-override)
204  {
205  NODELET_DEBUG("Unsubscribing from image topic.");
206  img_sub_.shutdown();
207  cam_sub_.shutdown();
208  }
209 
210 public:
211  virtual void onInit() // NOLINT(modernize-use-override)
212  {
213  Nodelet::onInit();
215 
216  pnh_->param("queue_size", queue_size_, 3);
217  pnh_->param("debug_view", debug_view_, false);
218  if (debug_view_)
219  {
220  always_subscribe_ = true;
221  }
222  prev_stamp_ = ros::Time(0, 0);
223 
224  window_name_ = "flow";
225 
226  reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
227  dynamic_reconfigure::Server<Config>::CallbackType f =
228  boost::bind(&FBackFlowNodelet::reconfigureCallback, this, _1, _2);
229  reconfigure_server_->setCallback(f);
230 
231  img_pub_ = advertiseImage(*pnh_, "image", 1);
232  msg_pub_ = advertise<opencv_apps::FlowArrayStamped>(*pnh_, "flows", 1);
233 
235  }
236 };
238 } // namespace opencv_apps
239 
240 namespace fback_flow
241 {
243 {
244 public:
245  virtual void onInit() // NOLINT(modernize-use-override)
246  {
247  ROS_WARN("DeprecationWarning: Nodelet fback_flow/fback_flow is deprecated, "
248  "and renamed to opencv_apps/fback_flow.");
250  }
251 };
252 } // namespace fback_flow
253 
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void doWork(const sensor_msgs::ImageConstPtr &msg, const std::string &input_frame_from_msg)
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
f
Nodelet to automatically subscribe/unsubscribe topics according to subscription of advertised topics...
Definition: nodelet.h:70
Demo code to calculate moments.
Definition: nodelet.h:48
#define NODELET_WARN_STREAM(...)
boost::shared_ptr< ros::NodeHandle > pnh_
Shared pointer to private nodehandle.
Definition: nodelet.h:250
void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &cam_info)
#define ROS_WARN(...)
boost::shared_ptr< ReconfigureServer > reconfigure_server_
void reconfigureCallback(Config &new_config, uint32_t level)
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
image_transport::CameraSubscriber cam_sub_
boost::shared_ptr< image_transport::ImageTransport > it_
void subscribe()
This method is called when publisher is subscribed by other nodes. Set up subscribers in this method...
bool always_subscribe_
A flag to disable watching mechanism and always subscribe input topics. It can be specified via ~alwa...
Definition: nodelet.h:273
virtual void onInitPostProcess()
Post processing of initialization of nodelet. You need to call this method in order to use always_sub...
Definition: nodelet.cpp:57
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
Definition: nodelet.cpp:40
void unsubscribe()
This method is called when publisher is unsubscribed by other nodes. Shut down subscribers in this me...
void publish(const sensor_msgs::Image &message) const
image_transport::Subscriber img_sub_
boost::shared_ptr< ros::NodeHandle > nh_
Shared pointer to nodehandle.
Definition: nodelet.h:245
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
image_transport::Publisher advertiseImage(ros::NodeHandle &nh, const std::string &topic, int queue_size)
Advertise an image topic and watch the publisher. Publishers which are created by this method...
Definition: nodelet.h:180
PLUGINLIB_EXPORT_CLASS(opencv_apps::FBackFlowNodelet, nodelet::Nodelet)
dynamic_reconfigure::Server< Config > ReconfigureServer
const std::string & frameWithDefault(const std::string &frame, const std::string &image_frame)
opencv_apps::FBackFlowConfig Config
static void trackbarCallback(int, void *)
#define NODELET_DEBUG(...)
static bool isSimTime()
sensor_msgs::ImagePtr toImageMsg() const
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
image_transport::Publisher img_pub_


opencv_apps
Author(s): Kei Okada
autogenerated on Sat Aug 22 2020 03:35:08