35 #include <boost/assign.hpp> 36 #include <boost/tuple/tuple.hpp> 39 #include <opencv2/opencv.hpp> 42 #include <Eigen/Dense> 49 DiagnosticNodelet::onInit();
50 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
51 dynamic_reconfigure::Server<Config>::CallbackType
f =
53 srv_->setCallback (f);
57 pub_ = advertise<sensor_msgs::Image>(*
pnh_,
"output", 1);
62 Config &config, uint32_t level)
77 async_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(
queue_size_);
82 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(
queue_size_);
86 names.push_back(
"~input/camera_info");
90 if (
pnh_->hasParam(
"image_height") &&
pnh_->hasParam(
"image_width"))
98 ROS_FATAL(
"Rosparam ~image_height and ~image_width must be set if ~use_camera_info=false");
111 const opencv_apps::FlowArrayStamped::ConstPtr& flow_msg)
117 const opencv_apps::FlowArrayStamped::ConstPtr& flow_msg,
118 const sensor_msgs::CameraInfo::ConstPtr& info_msg)
120 process(flow_msg, info_msg->height, info_msg->width);
124 const opencv_apps::FlowArrayStamped::ConstPtr& flow_msg,
125 const int image_height,
126 const int image_width)
130 cv::Mat mask = cv::Mat::zeros(image_height, image_width, CV_8UC1);
132 for (
size_t i=0; i<flow_msg->flow.size(); i++)
135 vel(0) = flow_msg->flow[i].velocity.x;
136 vel(1) = flow_msg->flow[i].velocity.y;
138 cv::Point center = cv::Point(flow_msg->flow[i].point.x, flow_msg->flow[i].point.y);
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
void publish(const boost::shared_ptr< M > &message) const
PLUGINLIB_EXPORT_CLASS(jsk_perception::FlowVelocityThresholding, nodelet::Nodelet)
jsk_perception::FlowVelocityThresholdingConfig Config
message_filters::Subscriber< opencv_apps::FlowArrayStamped > sub_flow_
virtual void configCallback(Config &config, uint32_t level)
std::vector< std::string > V_string
boost::shared_ptr< message_filters::Synchronizer< ApproximateSyncPolicy > > async_
virtual void callback(const opencv_apps::FlowArrayStamped::ConstPtr &flows_msg)
virtual void unsubscribe()
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
virtual void process(const opencv_apps::FlowArrayStamped::ConstPtr &flows_msg, const int image_height, const int image_width)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
sensor_msgs::ImagePtr toImageMsg() const
Connection registerCallback(const C &callback)
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_info_