46 #include <opencv2/highgui/highgui.hpp> 47 #include <opencv2/imgproc/imgproc.hpp> 48 #include <opencv2/video/background_segm.hpp> 50 #include <dynamic_reconfigure/server.h> 51 #include "opencv_apps/SegmentObjectsConfig.h" 52 #include "std_srvs/Empty.h" 53 #include "std_msgs/Float64.h" 54 #include "opencv_apps/Contour.h" 55 #include "opencv_apps/ContourArray.h" 56 #include "opencv_apps/ContourArrayStamped.h" 70 typedef opencv_apps::SegmentObjectsConfig
Config;
82 #ifndef CV_VERSION_EPOCH 94 const std::string&
frameWithDefault(
const std::string& frame,
const std::string& image_frame)
103 doWork(msg, cam_info->header.frame_id);
108 doWork(msg, msg->header.frame_id);
113 need_config_update_ =
true;
116 void doWork(
const sensor_msgs::ImageConstPtr& msg,
const std::string& input_frame_from_msg)
125 opencv_apps::ContourArrayStamped contours_msg;
126 contours_msg.header = msg->header;
129 cv::Mat bgmask, out_frame;
134 cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
135 if (need_config_update_)
137 reconfigure_server_->updateConfig(config_);
138 need_config_update_ =
false;
142 #ifndef CV_VERSION_EPOCH 143 bgsubtractor->apply(frame, bgmask, update_bg_model ? -1 : 0);
150 std::vector<std::vector<cv::Point> > contours;
151 std::vector<cv::Vec4i> hierarchy;
155 cv::dilate(bgmask, temp, cv::Mat(), cv::Point(-1, -1), niters);
156 cv::erode(temp, temp, cv::Mat(), cv::Point(-1, -1), niters * 2);
157 cv::dilate(temp, temp, cv::Mat(), cv::Point(-1, -1), niters);
159 cv::findContours(temp, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
161 out_frame = cv::Mat::zeros(frame.size(), CV_8UC3);
163 if (contours.empty())
168 int idx = 0, largest_comp = 0;
171 for (; idx >= 0; idx = hierarchy[idx][0])
173 const std::vector<cv::Point>& c = contours[idx];
174 double area = fabs(cv::contourArea(cv::Mat(c)));
181 cv::Scalar color(0, 0, 255);
182 cv::drawContours(out_frame, contours, largest_comp, color, CV_FILLED, 8, hierarchy);
184 std_msgs::Float64 area_msg;
185 area_msg.data = max_area;
186 for (
const std::vector<cv::Point>& contour : contours)
188 opencv_apps::Contour contour_msg;
189 for (
const cv::Point& j : contour)
191 opencv_apps::Point2D point_msg;
194 contour_msg.points.push_back(point_msg);
196 contours_msg.contours.push_back(contour_msg);
202 cv::imshow(window_name_, out_frame);
203 int keycode = cv::waitKey(1);
209 NODELET_INFO(
"Learn background is in state = %d", update_bg_model);
214 sensor_msgs::Image::Ptr out_img =
217 msg_pub_.
publish(contours_msg);
220 catch (cv::Exception& e)
222 NODELET_ERROR(
"Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
225 prev_stamp_ = msg->header.stamp;
228 bool updateBgModelCb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
231 NODELET_INFO(
"Learn background is in state = %d", update_bg_model);
238 if (config_.use_camera_info)
257 pnh_->param(
"queue_size", queue_size_, 3);
258 pnh_->param(
"debug_view", debug_view_,
false);
265 window_name_ =
"segmented";
266 update_bg_model =
true;
268 #ifndef CV_VERSION_EPOCH 269 bgsubtractor = cv::createBackgroundSubtractorMOG2();
271 bgsubtractor.set(
"noiseSigma", 10);
274 reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
275 dynamic_reconfigure::Server<Config>::CallbackType
f =
277 reconfigure_server_->setCallback(f);
280 msg_pub_ = advertise<opencv_apps::ContourArrayStamped>(*
pnh_,
"contours", 1);
281 area_pub_ = advertise<std_msgs::Float64>(*
pnh_,
"area", 1);
297 ROS_WARN(
"DeprecationWarning: Nodelet segment_objects/segment_objects is deprecated, " 298 "and renamed to opencv_apps/segment_objects.");
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
ros::ServiceServer update_bg_model_service_
Nodelet to automatically subscribe/unsubscribe topics according to subscription of advertised topics...
static bool need_config_update_
Demo code to calculate moments.
const std::string & frameWithDefault(const std::string &frame, const std::string &image_frame)
image_transport::CameraSubscriber cam_sub_
image_transport::Publisher img_pub_
void unsubscribe()
This method is called when publisher is unsubscribed by other nodes. Shut down subscribers in this me...
void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &cam_info)
boost::shared_ptr< ros::NodeHandle > pnh_
Shared pointer to private nodehandle.
void subscribe()
This method is called when publisher is subscribed by other nodes. Set up subscribers in this method...
image_transport::Subscriber img_sub_
bool always_subscribe_
A flag to disable watching mechanism and always subscribe input topics. It can be specified via ~alwa...
opencv_apps::SegmentObjectsConfig Config
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 publish(const sensor_msgs::Image &message) const
void doWork(const sensor_msgs::ImageConstPtr &msg, const std::string &input_frame_from_msg)
boost::shared_ptr< ros::NodeHandle > nh_
Shared pointer to nodehandle.
dynamic_reconfigure::Server< Config > ReconfigureServer
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...
#define NODELET_INFO(...)
PLUGINLIB_EXPORT_CLASS(opencv_apps::SegmentObjectsNodelet, nodelet::Nodelet)
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
static void trackbarCallback(int, void *)
bool updateBgModelCb(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
boost::shared_ptr< image_transport::ImageTransport > it_
boost::shared_ptr< ReconfigureServer > reconfigure_server_
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
#define NODELET_DEBUG(...)
sensor_msgs::ImagePtr toImageMsg() const
cv::Ptr< cv::BackgroundSubtractorMOG2 > bgsubtractor
void reconfigureCallback(Config &new_config, uint32_t level)