$search
00001 #ifndef IMAGEM_TRANSPORT_THEORA_PUBLISHER_H 00002 #define IMAGEM_TRANSPORT_THEORA_PUBLISHER_H 00003 00004 #include <message_transport/simple_publisher_plugin.h> 00005 #include <opencv/cv.h> 00006 #include <opencv/cxcore.h> 00007 #include <cv_bridge/cv_bridge.h> 00008 #include <std_msgs/Header.h> 00009 #include <dynamic_reconfigure/server.h> 00010 #include <theora_imagem_transport/TheoraPublisherConfig.h> 00011 #include <theora_image_transport/Packet.h> 00012 00013 #include <theora/codec.h> 00014 #include <theora/theoraenc.h> 00015 #include <theora/theoradec.h> 00016 00017 namespace theora_imagem_transport { 00018 00019 class TheoraPublisher : public message_transport::SimplePublisherPlugin<sensor_msgs::Image,theora_image_transport::Packet> 00020 { 00021 public: 00022 TheoraPublisher(); 00023 virtual ~TheoraPublisher(); 00024 00025 //Return the system unique string representing the theora transport type 00026 virtual std::string getTransportName() const 00027 { 00028 return "theora"; 00029 } 00030 00031 protected: 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 message_transport::SimplePublisherPlugin<sensor_msgs::Image,theora_image_transport::Packet>::PublishFn& publish_fn) const ; 00038 00039 private: 00040 // Overridden to tweak arguments and set up reconfigure server 00041 virtual void advertiseImpl(ros::NodeHandle & nh, const std::string& base_topic, uint32_t queue_size, 00042 const message_transport::SingleSubscriberPublisher<sensor_msgs::Image>::StatusCB& user_connect_cb, 00043 const message_transport::SingleSubscriberPublisher<sensor_msgs::Image>::StatusCB& user_disconnect_cb, 00044 const ros::VoidPtr& tracked_object, bool latch); 00045 00046 // Dynamic reconfigure support 00047 typedef theora_imagem_transport::TheoraPublisherConfig Config; 00048 typedef dynamic_reconfigure::Server<Config> ReconfigureServer; 00049 boost::shared_ptr<ReconfigureServer> reconfigure_server_; 00050 00051 void configCb(Config& config, uint32_t level); 00052 00053 // Utility functions 00054 bool ensureEncodingContext(const sensor_msgs::Image& image, const PublishFn& publish_fn) const; 00055 void oggPacketToMsg(const std_msgs::Header& header, const ogg_packet &oggpacket, 00056 theora_image_transport::Packet &msg) const; 00057 void updateKeyframeFrequency() const; 00058 00059 // Some data is preserved across calls to publish(), but from the user's perspective publish() is 00060 // "logically const" 00061 mutable cv_bridge::CvImage img_image_; 00062 mutable th_info encoder_setup_; 00063 mutable ogg_uint32_t keyframe_frequency_; 00064 mutable boost::shared_ptr<th_enc_ctx> encoding_context_; 00065 mutable std::vector<theora_image_transport::Packet> stream_header_; 00066 }; 00067 00068 } //namespace compressed_image_transport 00069 00070 #endif // IMAGEM_TRANSPORT_THEORA_PUBLISHER_H