$search
00001 #include <ros/ros.h> 00002 #include <sensor_msgs/Image.h> 00003 #include <message_transport/message_transport.h> 00004 #include <cv_bridge/cv_bridge.h> 00005 00006 class ImagePublisher { 00007 protected: 00008 00009 ros::NodeHandle n_; 00010 message_transport::MessageTransport<sensor_msgs::Image> it_; 00011 // sensor_msgs::CvBridge bridge_; 00012 cv_bridge::CvImagePtr cv_ptr; 00013 message_transport::Publisher image_pub_; 00014 sensor_msgs::Image image; 00015 00016 00017 public: 00018 00019 ImagePublisher(ros::NodeHandle &n) : n_(n), 00020 it_(n_,"imagem_transport","sensor_msgs::Image") { 00021 image_pub_ = it_.advertise("/image_source",1); 00022 cv_ptr.reset(new cv_bridge::CvImage); 00023 cv_ptr->image.create(cvSize(1500,1000),CV_8UC3); 00024 cv_ptr->toImageMsg(image); 00025 image.encoding = "bgr8"; 00026 } 00027 00028 ~ImagePublisher() 00029 { 00030 } 00031 00032 int mainloop() 00033 { 00034 00035 ros::Rate loop_rate(30); 00036 while (ros::ok()) 00037 { 00038 image.header.stamp = ros::Time::now(); 00039 image_pub_.publish(image); 00040 ROS_DEBUG("Published image at %f",image.header.stamp.toSec()); 00041 ros::spinOnce(); 00042 loop_rate.sleep(); 00043 } 00044 00045 return 0; 00046 } 00047 00048 }; 00049 00050 int main(int argc, char** argv) 00051 { 00052 ros::init(argc, argv, "test_publisher"); 00053 ros::NodeHandle n("~"); 00054 ImagePublisher ic(n); 00055 ic.mainloop(); 00056 return 0; 00057 } 00058