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
00023 virtual std::string getTransportName() const { return "theora"; }
00024
00025 protected:
00026
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
00033 virtual void connectCallback(const ros::SingleSubscriberPublisher& pub);
00034
00035
00036 virtual void publish(const sensor_msgs::Image& message,
00037 const PublishFn& publish_fn) const;
00038
00039
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
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
00053
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 }