36 #include <dynamic_reconfigure/server.h> 39 #include <sensor_msgs/CameraInfo.h> 40 #include <sensor_msgs/Image.h> 43 #include "image_proc/ResizeConfig.h" 51 std::shared_ptr<ros::NodeHandle>
nh_;
52 std::shared_ptr<ros::NodeHandle>
pnh_;
63 typedef image_proc::ResizeConfig
Config;
72 void imageCb(
const sensor_msgs::ImageConstPtr& image_msg);
73 void infoCb(
const sensor_msgs::CameraInfoConstPtr& info_msg);
75 void configCb(Config& config, uint32_t level);
87 ReconfigureServer::CallbackType
f =
97 pub_info_ =
pnh_->advertise<sensor_msgs::CameraInfo>(
"camera_info", 1, connect_cb, connect_cb);
136 sensor_msgs::CameraInfoPtr dst_info_msg(
new sensor_msgs::CameraInfo(*info_msg));
140 if (config.use_scale)
142 scale_y = config.scale_height;
143 scale_x = config.scale_width;
144 dst_info_msg->height =
static_cast<int>(info_msg->height * config.scale_height);
145 dst_info_msg->width =
static_cast<int>(info_msg->width * config.scale_width);
149 scale_y =
static_cast<double>(config.height) / info_msg->height;
150 scale_x = static_cast<double>(config.width) / info_msg->width;
151 dst_info_msg->height = config.height;
152 dst_info_msg->width = config.width;
155 dst_info_msg->K[0] = dst_info_msg->K[0] * scale_x;
156 dst_info_msg->K[2] = dst_info_msg->K[2] * scale_x;
157 dst_info_msg->K[4] = dst_info_msg->K[4] * scale_y;
158 dst_info_msg->K[5] = dst_info_msg->K[5] * scale_y;
160 dst_info_msg->P[0] = dst_info_msg->P[0] * scale_x;
161 dst_info_msg->P[2] = dst_info_msg->P[2] * scale_x;
162 dst_info_msg->P[3] = dst_info_msg->P[3] * scale_x;
163 dst_info_msg->P[5] = dst_info_msg->P[5] * scale_y;
164 dst_info_msg->P[6] = dst_info_msg->P[6] * scale_y;
184 ROS_ERROR(
"cv_bridge exception: %s", e.what());
188 if (config.use_scale)
190 cv::resize(cv_ptr->image,
scaled_cv_.
image, cv::Size(0, 0), config.scale_width, config.scale_height,
191 config.interpolation);
195 int height = config.height == -1 ? image_msg->height : config.height;
196 int width = config.width == -1 ? image_msg->width : config.width;
197 cv::resize(cv_ptr->image,
scaled_cv_.
image, cv::Size(width, height), 0, 0, config.interpolation);
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
cv_bridge::CvImage scaled_cv_
std::mutex connect_mutex_
void imageCb(const sensor_msgs::ImageConstPtr &image_msg)
uint32_t getNumSubscribers() const
ros::NodeHandle & getNodeHandle() const
std::shared_ptr< image_transport::ImageTransport > private_it_
image_transport::Subscriber sub_image_
void infoCb(const sensor_msgs::CameraInfoConstPtr &info_msg)
ros::NodeHandle & getPrivateNodeHandle() const
std::shared_ptr< ReconfigureServer > reconfigure_server_
std::shared_ptr< image_transport::ImageTransport > it_
void configCb(Config &config, uint32_t level)
dynamic_reconfigure::Server< Config > ReconfigureServer
sensor_msgs::ImagePtr toImageMsg() const
void publish(const boost::shared_ptr< M > &message) const
PLUGINLIB_EXPORT_CLASS(image_proc::CropNonZeroNodelet, nodelet::Nodelet)
ros::Subscriber sub_info_
std::shared_ptr< ros::NodeHandle > nh_
image_proc::ResizeConfig Config
void publish(const sensor_msgs::Image &message) const
image_transport::Publisher pub_image_
uint32_t getNumSubscribers() const
std::shared_ptr< ros::NodeHandle > pnh_