2 #include <boost/shared_ptr.hpp> 3 #include <boost/assign.hpp> 11 #include <jsk_recognition_msgs/SparseImage.h> 32 do_work(msg, msg->header.frame_id);
35 void do_work(
const sensor_msgs::ImageConstPtr& msg,
const std::string input_frame_from_msg){
37 int channel = enc::numChannels(msg->encoding);
40 bool useData32 =
false;
41 if (std::max(msg->width, msg->height) > 256){
46 _spr_img_ptr->header.stamp = msg->header.stamp;
47 _spr_img_ptr->header.frame_id = input_frame_from_msg;
48 _spr_img_ptr->width = msg->width;
49 _spr_img_ptr->height = msg->height;
50 if (!_spr_img_ptr->data16.empty()) _spr_img_ptr->data16.clear();
51 if (!_spr_img_ptr->data32.empty()) _spr_img_ptr->data32.clear();
54 for (uint32_t
y = 0;
y < msg->height; ++
y){
55 for (uint32_t
x = 0;
x < msg->width; ++
x){
56 if(msg->data[
x*channel+
y*msg->step] > 125){
57 if (useData32) _spr_img_ptr->data32.push_back( (
x << 16) |
y );
58 else _spr_img_ptr->data16.push_back( (
x << 8) |
y );
64 if (_print_point_num) {
66 if (useData32) size = _spr_img_ptr->data32.size();
67 else size = _spr_img_ptr->data16.size();
68 NODELET_INFO(
"%d point encoded. encoded area per is %lf%%.", size, (100 * ((
double) size)/(msg->height* msg->width)));
72 _spr_img_pub.
publish(*_spr_img_ptr);
95 if (_subscriber_count++ == 0) {
102 if (_subscriber_count == 0) {
112 _subscriber_count = 0;
115 _spr_img_pub = _nh.
advertise<jsk_recognition_msgs::SparseImage>(
"sparse_image", 10, connect_cb, disconnect_cb);
116 _spr_img_ptr = boost::make_shared<jsk_recognition_msgs::SparseImage>();
117 _ln.
param(
"rate", _rate, 3.0);
118 _ln.
param(
"print_point_num", _print_point_num,
false);
void do_work(const sensor_msgs::ImageConstPtr &msg, const std::string input_frame_from_msg)
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
#define NODELET_ERROR(...)
boost::shared_ptr< image_transport::ImageTransport > _it
void publish(const boost::shared_ptr< M > &message) const
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
jsk_recognition_msgs::SparseImagePtr _spr_img_ptr
void connectCb(const ros::SingleSubscriberPublisher &ssp)
std::vector< std::string > V_string
image_transport::Subscriber _img_sub
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void disconnectCb(const ros::SingleSubscriberPublisher &)
PLUGINLIB_EXPORT_CLASS(jsk_perception::SparseImageEncoder, nodelet::Nodelet)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle & getNodeHandle() const
#define NODELET_INFO(...)
#define NODELET_DEBUG(...)
ros::Publisher _spr_img_pub