47 #include <opencv2/highgui/highgui.hpp> 48 #include <opencv2/imgproc/imgproc.hpp> 49 #include <opencv2/video/tracking.hpp> 51 #include <dynamic_reconfigure/server.h> 52 #include "opencv_apps/FBackFlowConfig.h" 53 #include "opencv_apps/FlowArrayStamped.h" 66 typedef opencv_apps::FBackFlowConfig
Config;
85 const std::string&
frameWithDefault(
const std::string& frame,
const std::string& image_frame)
92 void imageCallbackWithInfo(
const sensor_msgs::ImageConstPtr& msg,
const sensor_msgs::CameraInfoConstPtr& cam_info)
94 doWork(msg, cam_info->header.frame_id);
99 doWork(msg, msg->header.frame_id);
104 need_config_update_ =
true;
107 void doWork(
const sensor_msgs::ImageConstPtr& msg,
const std::string& input_frame_from_msg)
116 opencv_apps::FlowArrayStamped flows_msg;
117 flows_msg.header = msg->header;
121 cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
122 if (need_config_update_)
124 reconfigure_server_->updateConfig(config_);
125 need_config_update_ =
false;
132 NODELET_WARN_STREAM(
"Detected jump back in time of " << msg->header.stamp <<
". Clearing optical flow cache.");
133 prevgray = cv::Mat();
137 if (frame.channels() > 1)
139 cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
147 cv::calcOpticalFlowFarneback(prevgray, gray, flow, 0.5, 3, 15, 3, 5, 1.2, 0);
148 cv::cvtColor(prevgray, cflow, cv::COLOR_GRAY2BGR);
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)
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);
159 opencv_apps::Flow flow_msg;
160 opencv_apps::Point2D point_msg;
161 opencv_apps::Point2D velocity_msg;
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);
172 std::swap(prevgray, gray);
177 cv::imshow(window_name_, cflow);
178 int c = cv::waitKey(1);
186 catch (cv::Exception& e)
188 NODELET_ERROR(
"Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
191 prev_stamp_ = msg->header.stamp;
197 if (config_.use_camera_info)
216 pnh_->param(
"queue_size", queue_size_, 3);
217 pnh_->param(
"debug_view", debug_view_,
false);
224 window_name_ =
"flow";
226 reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
227 dynamic_reconfigure::Server<Config>::CallbackType
f =
229 reconfigure_server_->setCallback(f);
232 msg_pub_ = advertise<opencv_apps::FlowArrayStamped>(*
pnh_,
"flows", 1);
247 ROS_WARN(
"DeprecationWarning: Nodelet fback_flow/fback_flow is deprecated, " 248 "and renamed to opencv_apps/fback_flow.");
254 #ifdef USE_PLUGINLIB_CLASS_LIST_MACROS_H 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(...)
Nodelet to automatically subscribe/unsubscribe topics according to subscription of advertised topics...
Demo code to calculate moments.
#define NODELET_WARN_STREAM(...)
boost::shared_ptr< ros::NodeHandle > pnh_
Shared pointer to private nodehandle.
void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &cam_info)
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...
sensor_msgs::ImagePtr toImageMsg() const
bool always_subscribe_
A flag to disable watching mechanism and always subscribe input topics. It can be specified via ~alwa...
void publish(const boost::shared_ptr< M > &message) const
virtual void onInitPostProcess()
Post processing of initialization of nodelet. You need to call this method in order to use always_sub...
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
void unsubscribe()
This method is called when publisher is unsubscribed by other nodes. Shut down subscribers in this me...
image_transport::Subscriber img_sub_
boost::shared_ptr< ros::NodeHandle > nh_
Shared pointer to nodehandle.
void publish(const sensor_msgs::Image &message) const
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...
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)
static bool need_config_update_
opencv_apps::FBackFlowConfig Config
static void trackbarCallback(int, void *)
#define NODELET_DEBUG(...)
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
image_transport::Publisher img_pub_