$search
00001 #include <image_transport/simple_publisher_plugin.h> 00002 #include <opencv/cv.h> 00003 #include <opencv/cxcore.h> 00004 #include <cv_bridge/CvBridge.h> 00005 #include <dynamic_reconfigure/server.h> 00006 #include <theora_image_transport/TheoraPublisherConfig.h> 00007 #include <theora_image_transport/Packet.h> 00008 00009 #include <theora/codec.h> 00010 #include <theora/theoraenc.h> 00011 #include <theora/theoradec.h> 00012 00013 namespace theora_image_transport { 00014 00015 class TheoraPublisher : public image_transport::SimplePublisherPlugin<theora_image_transport::Packet> 00016 { 00017 public: 00018 TheoraPublisher(); 00019 00020 ~TheoraPublisher(); 00021 00022 // Return the system unique string representing the theora transport type 00023 virtual std::string getTransportName() const { return "theora"; } 00024 00025 protected: 00026 // Overridden to tweak arguments and set up reconfigure server 00027 virtual void advertiseImpl(ros::NodeHandle &nh, const std::string &base_topic, uint32_t queue_size, 00028 const image_transport::SubscriberStatusCallback &user_connect_cb, 00029 const image_transport::SubscriberStatusCallback &user_disconnect_cb, 00030 const ros::VoidPtr &tracked_object, bool latch); 00031 00032 // Callback to send header packets to new clients 00033 virtual void connectCallback(const ros::SingleSubscriberPublisher& pub); 00034 00035 // Main publish function 00036 virtual void publish(const sensor_msgs::Image& message, 00037 const PublishFn& publish_fn) const; 00038 00039 // Dynamic reconfigure support 00040 typedef theora_image_transport::TheoraPublisherConfig Config; 00041 typedef dynamic_reconfigure::Server<Config> ReconfigureServer; 00042 boost::shared_ptr<ReconfigureServer> reconfigure_server_; 00043 00044 void configCb(Config& config, uint32_t level); 00045 00046 // Utility functions 00047 bool ensureEncodingContext(const sensor_msgs::Image& image, const PublishFn& publish_fn) const; 00048 void oggPacketToMsg(const roslib::Header& header, const ogg_packet &oggpacket, 00049 theora_image_transport::Packet &msg) const; 00050 void updateKeyframeFrequency() const; 00051 00052 // Some data is preserved across calls to publish(), but from the user's perspective publish() is 00053 // "logically const" 00054 mutable sensor_msgs::CvBridge img_bridge_; 00055 mutable th_info encoder_setup_; 00056 mutable ogg_uint32_t keyframe_frequency_; 00057 mutable boost::shared_ptr<th_enc_ctx> encoding_context_; 00058 mutable std::vector<theora_image_transport::Packet> stream_header_; 00059 }; 00060 00061 } //namespace compressed_image_transport