├── .gitignore
├── README.md
├── include
    └── ros_h264_streamer
    │   ├── h264_api.h
    │   ├── h264_decoder.h
    │   ├── h264_streamer.h
    │   ├── h264_encoder.h
    │   └── h264_receiver.h
├── src
    ├── private
    │   └── net_buffer_size.h
    ├── utils
    │   ├── output
    │   │   ├── output.h
    │   │   ├── matroska_ebml.h
    │   │   ├── matroska.c
    │   │   └── matroska_ebml.c
    │   └── mkv_recorder.cpp
    ├── h264_decoder.cpp
    ├── h264_encoder.cpp
    ├── h264_streamer.cpp
    └── h264_receiver.cpp
├── package.xml
├── test
    ├── testEncoderDecoder.cpp
    ├── testStreamer.cpp
    └── testReceiver.cpp
└── CMakeLists.txt


/.gitignore:
--------------------------------------------------------------------------------
1 | *.swp
2 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | ros_h264_streamer
2 | =================
3 | 
4 | A simple ROS node to stream/receive h.264 encoded images through a UDP/TCP socket
5 | 


--------------------------------------------------------------------------------
/include/ros_h264_streamer/h264_api.h:
--------------------------------------------------------------------------------
1 | #ifndef _H_H264_API_H_
2 | #define _H_H264_API_H_
3 |   #ifdef WIN32
4 |     #define H264_API __declspec(dllexport)
5 |   #else
6 |     #define H264_API
7 |   #endif
8 | #endif


--------------------------------------------------------------------------------
/src/private/net_buffer_size.h:
--------------------------------------------------------------------------------
 1 | #ifndef _H_NETWORK_BUFFER_SIZE_H_
 2 | #define _H_NETWORK_BUFFER_SIZE_H_
 3 | 
 4 | namespace ros_h264_streamer_private
 5 | {
 6 | 
 7 | static int _udp_video_chunk_size = 1000;
 8 | static int _tcp_video_chunk_size = 256;
 9 | static int _request_size = 1024;
10 | 
11 | } // namespace private_ros_h264_streamer
12 | 
13 | #endif
14 | 


--------------------------------------------------------------------------------
/include/ros_h264_streamer/h264_decoder.h:
--------------------------------------------------------------------------------
 1 | #ifndef _H_ROS_H264_DECODER_H_
 2 | #define _H_ROS_H264_DECODER_H_
 3 | 
 4 | #include <ros_h264_streamer/h264_api.h>
 5 | 
 6 | #include <sensor_msgs/Image.h>
 7 | 
 8 | namespace ros_h264_streamer
 9 | {
10 | 
11 | struct H264DecoderImpl;
12 | 
13 | class H264_API H264Decoder
14 | {
15 | public:
16 |   H264Decoder(int width, int height);
17 | 
18 |   /* Decode data into a sensor_msgs::ImagePtr, the header is untouched */
19 |   int decode(int frame_size, uint8_t * frame_data, sensor_msgs::ImagePtr & out);
20 | private:
21 |   boost::shared_ptr<H264DecoderImpl> impl;
22 | };
23 | 
24 | } // ros_h264_streamer
25 | 
26 | #endif
27 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <package>
 2 |   <name>ros_h264_streamer</name>
 3 |   <author email="pierre.gergondet+github@gmail.com">Pierre Gergondet</author>
 4 |   <maintainer email="pierre.gergondet+github@gmail.com">Pierre Gergondet</maintainer>
 5 |   <version>0.1.0</version>
 6 |   <description>
 7 |     A simple ROS node to stream/receive h.264 encoded images through a UDP/TCP socket
 8 |   </description>
 9 |   <license>BSD</license>
10 |   <url>http://github.com/gergondet/ros_h264_streamer</url>
11 | 
12 |   <buildtool_depend>catkin</buildtool_depend>
13 | 
14 |   <build_depend>roscpp</build_depend>
15 |   <build_depend>image_transport</build_depend>
16 |   <build_depend>cv_bridge</build_depend>
17 | 
18 |   <run_depend>roscpp</run_depend>
19 |   <run_depend>image_transport</run_depend>
20 |   <run_depend>cv_bridge</run_depend>
21 | </package>
22 | 


--------------------------------------------------------------------------------
/include/ros_h264_streamer/h264_streamer.h:
--------------------------------------------------------------------------------
 1 | #ifndef _H_ROS_H264_STREAMER_H_
 2 | #define _H_ROS_H264_STREAMER_H_
 3 | 
 4 | #include <string>
 5 | #include <boost/shared_ptr.hpp>
 6 | #include <ros/ros.h>
 7 | 
 8 | namespace ros_h264_streamer
 9 | {
10 | 
11 | struct H264StreamerImpl;
12 | 
13 | 
14 | class H264Streamer
15 | {
16 | public:
17 |   struct Config
18 |   {
19 |     Config()
20 |     : udp(true), server(true),
21 |       port(10000), host("127.0.0.1"),
22 |       camera_topic("/camera/rgb/image_raw"),
23 |       quality(28), fps_num(30), fps_den(1)
24 |     {}
25 |     bool udp;
26 |     bool server;
27 |     short port;
28 |     std::string host;
29 |     std::string camera_topic;
30 |     int quality;
31 |     int fps_num;
32 |     int fps_den;
33 |   };
34 | 
35 |   H264Streamer(H264Streamer::Config & conf, ros::NodeHandle & nh);
36 | private:
37 |   boost::shared_ptr<H264StreamerImpl> impl;
38 | };
39 | 
40 | } // namespace ros_h264_streamer
41 | 
42 | #endif
43 | 


--------------------------------------------------------------------------------
/include/ros_h264_streamer/h264_encoder.h:
--------------------------------------------------------------------------------
 1 | #ifndef _H_ROS_H264_ENCODER_H_
 2 | #define _H_ROS_H264_ENCODER_H_
 3 | 
 4 | #include <ros_h264_streamer/h264_api.h>
 5 | #include <sensor_msgs/Image.h>
 6 | #include <stdint.h>
 7 | 
 8 | struct x264_param_t;
 9 | struct x264_t;
10 | 
11 | namespace ros_h264_streamer
12 | {
13 | 
14 | struct H264_API H264EncoderResult
15 | {
16 |   H264EncoderResult() : frame_size(0), frame_data(0) {}
17 | 
18 |   int frame_size;
19 |   uint8_t * frame_data;
20 | };
21 | 
22 | struct H264EncoderImpl;
23 | 
24 | class H264_API H264Encoder
25 | {
26 | public:
27 |   /* Quality can scale from 1 to 100, 1 being (very) high-quality, 100 being (very) low */
28 |   /* FPS is given as a fraction */
29 |   H264Encoder(int width, int height, int quality_level, int fps_num, int fps_den, const std::string & encoding, bool streaming = true);
30 | 
31 |   H264EncoderResult encode(const sensor_msgs::ImageConstPtr & img, uint64_t pts = 0);
32 | 
33 |   x264_param_t * GetParameters();
34 | 
35 |   x264_t * GetEncoder();
36 | 
37 |   /* Actual type: x264_picture_t */
38 |   void * GetPicIn();
39 | 
40 |   /* Actual type: x264_picture_t */
41 |   void * GetPicOut();
42 | private:
43 |   boost::shared_ptr<H264EncoderImpl> impl;
44 | };
45 | 
46 | } // namespace ros_h264_streamer
47 | 
48 | #endif
49 | 


--------------------------------------------------------------------------------
/include/ros_h264_streamer/h264_receiver.h:
--------------------------------------------------------------------------------
 1 | #ifndef _H_ROS_H264_RECEIVER_H_
 2 | #define _H_ROS_H264_RECEIVER_H_
 3 | 
 4 | #include <ros_h264_streamer/h264_api.h>
 5 | 
 6 | #include <string>
 7 | #include <boost/shared_ptr.hpp>
 8 | #ifndef WIN32
 9 | #include <ros/ros.h>
10 | #endif
11 | #include <sensor_msgs/Image.h>
12 | 
13 | namespace ros_h264_streamer
14 | {
15 | 
16 | struct H264ReceiverImpl;
17 | 
18 | 
19 | class H264_API H264Receiver
20 | {
21 | public:
22 |   struct Config
23 |   {
24 |     Config()
25 |     : width(640), height(480),
26 |       udp(true), server(true),
27 |       port(10000), host("127.0.0.1"),
28 |       publish(false), publish_topic("/ros_h264_receiver/rgb/image_raw"), frame_id("/ros_h264_receiver_rgb_optical_frame")
29 | 
30 |     {}
31 |     int width;
32 |     int height;
33 |     bool udp;
34 |     bool server;
35 |     short port;
36 |     std::string host;
37 |     bool publish;
38 |     std::string publish_topic;
39 |     std::string frame_id;
40 |   };
41 | 
42 |   #ifndef WIN32
43 |   H264Receiver(H264Receiver::Config & conf, ros::NodeHandle & nh);
44 |   #else
45 |   H264Receiver(H264Receiver::Config & conf);
46 |   #endif
47 | 
48 |   /* Returns true if we are returning a new image compared to last call */
49 |   bool getLatestImage(sensor_msgs::ImagePtr & img);
50 | private:
51 |   boost::shared_ptr<H264ReceiverImpl> impl;
52 | };
53 | 
54 | } // namespace ros_h264_Receiver
55 | 
56 | #endif
57 | 


--------------------------------------------------------------------------------
/test/testEncoderDecoder.cpp:
--------------------------------------------------------------------------------
 1 | #include <iostream>
 2 | #include <ros_h264_streamer/h264_encoder.h>
 3 | #include <ros_h264_streamer/h264_decoder.h>
 4 | 
 5 | #include <opencv/highgui.h>
 6 | #include <cv_bridge/cv_bridge.h>
 7 | 
 8 | int main(int argc, char * argv[])
 9 | {
10 |   if(argc < 2)
11 |   {
12 |     std::cerr << "[Usage] " << argv[0] << " image_file" << std::endl;
13 |     return 0;
14 |   }
15 | 
16 |   ros::Time::init();
17 | 
18 |   std_msgs::Header header;
19 |   cv_bridge::CvImage cvmsg(header, "bgr8", cv::imread(argv[1]));
20 |   sensor_msgs::ImagePtr msg = cvmsg.toImageMsg();
21 | 
22 |   std::cout << "Image loaded, image size: " << msg->width << "x" << msg->height << ", encoding: " << msg->encoding << std::endl;
23 | 
24 |   ros_h264_streamer::H264Encoder encoder(msg->width, msg->height, 20, 30, 1, msg->encoding);
25 |   ros::Time before_encoding = ros::Time::now();
26 |   ros_h264_streamer::H264EncoderResult res = encoder.encode(msg);
27 |   ros::Time after_encoding = ros::Time::now();
28 | 
29 |   ros::Duration encoding_duration = after_encoding - before_encoding;
30 |   std::cout << "Image encoded, encoded size is " << res.frame_size << std::endl;
31 |   std::cout << "Encoding took " << encoding_duration << std::endl;
32 | 
33 |   sensor_msgs::ImagePtr out(new sensor_msgs::Image);
34 |   ros_h264_streamer::H264Decoder decoder(msg->width, msg->height);
35 | 
36 |   ros::Time before_decoding = ros::Time::now();
37 |   int len = decoder.decode(res.frame_size, res.frame_data, out);
38 |   ros::Time after_decoding = ros::Time::now();
39 | 
40 |   std::cout << "Image decoded, decoded image size is: " << out->width << "x" << out->height << std::endl;
41 |   std::cout << "Decoding took " << (after_decoding - before_decoding) << std::endl;
42 | 
43 |   cv_bridge::CvImagePtr cvout = cv_bridge::toCvCopy(out);
44 |   std::cout << "Saving to disk" << std::endl;
45 |   cv::imwrite("testEncoderDecoderResult.jpg", cvout->image);
46 | 
47 |   double fps = (double)encoding_duration.toNSec();
48 |   fps = floor(1e9/fps);
49 |   std::cout << "Could encode at (roughly) " << fps << "FPS" << std::endl;
50 | 
51 |   return 0;
52 | }
53 | 


--------------------------------------------------------------------------------
/src/utils/output/output.h:
--------------------------------------------------------------------------------
 1 | /*****************************************************************************
 2 |  * output.h: x264 file output modules
 3 |  *****************************************************************************
 4 |  * Copyright (C) 2003-2012 x264 project
 5 |  *
 6 |  * Authors: Laurent Aimar <fenrir@via.ecp.fr>
 7 |  *          Loren Merritt <lorenm@u.washington.edu>
 8 |  *
 9 |  * This program is free software; you can redistribute it and/or modify
10 |  * it under the terms of the GNU General Public License as published by
11 |  * the Free Software Foundation; either version 2 of the License, or
12 |  * (at your option) any later version.
13 |  *
14 |  * This program is distributed in the hope that it will be useful,
15 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17 |  * GNU General Public License for more details.
18 |  *
19 |  * You should have received a copy of the GNU General Public License
20 |  * along with this program; if not, write to the Free Software
21 |  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02111, USA.
22 |  *
23 |  * This program is also available under a commercial proprietary license.
24 |  * For more information, contact us at licensing@x264.com.
25 |  *****************************************************************************/
26 | 
27 | #ifndef X264_OUTPUT_H
28 | #define X264_OUTPUT_H
29 | 
30 | #include <inttypes.h>
31 | #include <stdint.h>
32 | #include <stdio.h>
33 | #include <stdlib.h>
34 | 
35 | #include <x264.h>
36 | 
37 | typedef void* hnd_t;
38 | 
39 | typedef struct
40 | {
41 |     int use_dts_compress;
42 | } cli_output_opt_t;
43 | 
44 | typedef struct
45 | {
46 |     int (*open_file)( char *psz_filename, hnd_t *p_handle, cli_output_opt_t *opt );
47 |     int (*set_param)( hnd_t handle, x264_param_t *p_param );
48 |     int (*write_headers)( hnd_t handle, x264_nal_t *p_nal );
49 |     int (*write_frame)( hnd_t handle, uint8_t *p_nal, int i_size, x264_picture_t *p_picture );
50 |     int (*close_file)( hnd_t handle, int64_t largest_pts, int64_t second_largest_pts );
51 | } cli_output_t;
52 | 
53 | extern const cli_output_t mkv_output;
54 | 
55 | #endif
56 | 


--------------------------------------------------------------------------------
/test/testStreamer.cpp:
--------------------------------------------------------------------------------
 1 | #include <ros_h264_streamer/h264_streamer.h>
 2 | 
 3 | #include <boost/program_options.hpp>
 4 | namespace po = boost::program_options;
 5 | 
 6 | int main(int argc, char * argv[])
 7 | {
 8 |   ros::init(argc, argv, "ros_h264_streamer_test_streamer");
 9 |   ros::NodeHandle nh;
10 | 
11 |   ros_h264_streamer::H264Streamer::Config conf;
12 |   conf.udp = false;
13 |   conf.server = false;
14 |   conf.port = 10000;
15 |   conf.host = "127.0.0.1";
16 |   conf.camera_topic = "/camera/rgb/image_raw";
17 | 
18 |   po::options_description desc("testStreamer options");
19 |   desc.add_options()
20 |     ("help", "display help message")
21 |     ("udp,u", po::value<bool>(&conf.udp)->default_value("udp"), "connection type; valid values are tcp and udp")
22 |     ("server,s", po::value<bool>(&conf.server)->default_value(false), "is this a server?")
23 |     ("port,p", po::value<short>(&conf.port)->default_value(10000), "connection port")
24 |     ("host,h", po::value<std::string>(&conf.host)->default_value("127.0.0.1"), "connection host (irrelevant for server mode")
25 |     ("quality,q", po::value<int>(&conf.quality)->default_value(28), "Encoding quality setting, range from 1 (highest quality) to 100 (lowest quality)")
26 |     ("low-quality", "Set video quality to a low setting")
27 |     ("high-quality", "Set video quality to a high setting")
28 |     ("topic,t", po::value<std::string>(&conf.camera_topic)->default_value("/camera/rgb/image_raw"), "camera topic to stream")
29 |     ("fps_num", po::value<int>(&conf.fps_num)->default_value(30), "FPS numerator value, will be also interpreted as camera real fps")
30 |     ("fps_den", po::value<int>(&conf.fps_den)->default_value(1), "FPS denominator value, will be interpreted as sub-sampling denominator");
31 | 
32 |   po::variables_map vm;
33 |   po::store(po::parse_command_line(argc, argv, desc), vm);
34 |   po::notify(vm);
35 | 
36 |   if(vm.count("help"))
37 |   {
38 |     std::cout << desc << std::endl;
39 |     return 1;
40 |   }
41 |   if(vm.count("low-quality"))
42 |   {
43 |     conf.quality = 35;
44 |   }
45 |   if(vm.count("high-quality"))
46 |   {
47 |     conf.quality = 20;
48 |   }
49 | 
50 |   ros_h264_streamer::H264Streamer streamer(conf, nh);
51 | 
52 |   while(ros::ok())
53 |   {
54 |     ros::spin();
55 |   }
56 | 
57 |   return 0;
58 | }
59 | 


--------------------------------------------------------------------------------
/src/utils/output/matroska_ebml.h:
--------------------------------------------------------------------------------
 1 | /*****************************************************************************
 2 |  * matroska_ebml.h: matroska muxer utilities
 3 |  *****************************************************************************
 4 |  * Copyright (C) 2005-2012 x264 project
 5 |  *
 6 |  * Authors: Mike Matsnev <mike@haali.su>
 7 |  *
 8 |  * This program is free software; you can redistribute it and/or modify
 9 |  * it under the terms of the GNU General Public License as published by
10 |  * the Free Software Foundation; either version 2 of the License, or
11 |  * (at your option) any later version.
12 |  *
13 |  * This program is distributed in the hope that it will be useful,
14 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16 |  * GNU General Public License for more details.
17 |  *
18 |  * You should have received a copy of the GNU General Public License
19 |  * along with this program; if not, write to the Free Software
20 |  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02111, USA.
21 |  *
22 |  * This program is also available under a commercial proprietary license.
23 |  * For more information, contact us at licensing@x264.com.
24 |  *****************************************************************************/
25 | 
26 | #ifndef X264_MATROSKA_EBML_H
27 | #define X264_MATROSKA_EBML_H
28 | 
29 | /* Matroska display size units from the spec */
30 | #define	DS_PIXELS        0
31 | #define	DS_CM            1
32 | #define	DS_INCHES        2
33 | #define	DS_ASPECT_RATIO  3
34 | 
35 | typedef struct mk_writer mk_writer;
36 | 
37 | mk_writer *mk_create_writer( const char *filename );
38 | 
39 | int mk_write_header( mk_writer *w, const char *writing_app,
40 |                      const char *codec_id,
41 |                      const void *codec_private, unsigned codec_private_size,
42 |                      int64_t default_frame_duration,
43 |                      int64_t timescale,
44 |                      unsigned width, unsigned height,
45 |                      unsigned d_width, unsigned d_height, int display_size_units );
46 | 
47 | int mk_start_frame( mk_writer *w );
48 | int mk_add_frame_data( mk_writer *w, const void *data, unsigned size );
49 | int mk_set_frame_flags( mk_writer *w, int64_t timestamp, int keyframe, int skippable );
50 | int mk_close( mk_writer *w, int64_t last_delta );
51 | 
52 | #endif
53 | 


--------------------------------------------------------------------------------
/test/testReceiver.cpp:
--------------------------------------------------------------------------------
 1 | #include <ros_h264_streamer/h264_receiver.h>
 2 | 
 3 | #include <boost/program_options.hpp>
 4 | namespace po = boost::program_options;
 5 | #include <iomanip>
 6 | 
 7 | int main(int argc, char * argv[])
 8 | {
 9 |   #ifndef WIN32
10 |   ros::init(argc, argv, "ros_h264_streamer_test_receiver");
11 |   ros::NodeHandle nh;
12 |   #endif
13 | 
14 |   ros_h264_streamer::H264Receiver::Config conf;
15 |   conf.width = 640; conf.height = 480;
16 |   conf.udp = false;
17 |   conf.server = false;
18 |   conf.port = 10000;
19 |   conf.host = "127.0.0.1";
20 | 
21 |   conf.publish = true;
22 |   conf.publish_topic = "/ros_h264_receiver/rgb/image_raw";
23 | 
24 |   po::options_description desc("testStreamer options");
25 |   desc.add_options()
26 |     ("help", "display help message")
27 |     ("width", po::value<int>(&conf.width)->default_value(640), "image width")
28 |     ("height", po::value<int>(&conf.height)->default_value(480), "image height")
29 |     ("udp,u", po::value<bool>(&conf.udp)->default_value("udp"), "connection type; valid values are tcp and udp")
30 |     ("server,s", po::value<bool>(&conf.server)->default_value(false), "is this a server?")
31 |     ("port,p", po::value<short>(&conf.port)->default_value(10000), "connection port")
32 |     ("host,h", po::value<std::string>(&conf.host)->default_value("127.0.0.1"), "connection host (irrelevant for server mode");
33 | 
34 |   po::variables_map vm;
35 |   po::store(po::parse_command_line(argc, argv, desc), vm);
36 |   po::notify(vm);
37 | 
38 |   if(vm.count("help"))
39 |   {
40 |     std::cout << desc << std::endl;
41 |     return 1;
42 |   }
43 | 
44 |   #ifndef WIN32
45 |   ros_h264_streamer::H264Receiver receiver(conf, nh);
46 |   #else
47 |   ros_h264_streamer::H264Receiver receiver(conf);
48 |   #endif
49 | 
50 |   #ifndef WIN32
51 |   ros::Time tin = ros::Time::now();
52 |   sensor_msgs::ImagePtr img(new sensor_msgs::Image);
53 |   unsigned int frames_in = 0;
54 |   while(ros::ok())
55 |   {
56 |     if(receiver.getLatestImage(img))
57 |     {
58 |       frames_in++;
59 |       if(frames_in == 10)
60 |       {
61 |         ros::Time tout = ros::Time::now();
62 |         ros::Duration dt = tout - tin;
63 |         tin = tout;
64 |         std::cout << "\rReceiving at " << 1e10/dt.toNSec() << "Hz" << std::flush;
65 |         frames_in = 0;
66 |       }
67 |     }
68 |   }
69 |   #else
70 |   sensor_msgs::ImagePtr img(new sensor_msgs::Image);
71 |   long long ptick = 0;
72 |   long long tick = 0;
73 |   long long qPerfF = 0;
74 |   QueryPerformanceFrequency((LARGE_INTEGER*)&qPerfF);
75 |   double f = 0;
76 |   unsigned int fc = 0;
77 |   while(1)
78 |   {
79 |     if(receiver.getLatestImage(img))
80 |     {
81 |         if(fc == 0)
82 |         {
83 |             QueryPerformanceCounter((LARGE_INTEGER*)&tick);
84 |             f = 10*(double)qPerfF/((double)tick - (double)ptick);
85 |             std::cout << "\rCurrent frequency: " << f << "Hz" << std::flush;
86 |             ptick = tick;
87 |         }
88 |         ++fc;
89 |         if(fc == 10) fc = 0;
90 |     }
91 |   }
92 |   #endif
93 | 
94 |   return 0;
95 | }
96 | 


--------------------------------------------------------------------------------
/src/h264_decoder.cpp:
--------------------------------------------------------------------------------
  1 | #include <ros_h264_streamer/h264_decoder.h>
  2 | 
  3 | #include <sensor_msgs/image_encodings.h>
  4 | 
  5 | #include <stdint.h>
  6 | #ifndef UINT64_C
  7 | typedef uint64_t UINT64_C;
  8 | #endif
  9 | 
 10 | extern "C"
 11 | {
 12 | #include "libswscale/swscale.h"
 13 | #include "libavutil/imgutils.h"
 14 | #include "libavutil/mem.h"
 15 | #include "libavutil/opt.h"
 16 | #include "libavcodec/avcodec.h"
 17 | #include "libavutil/mathematics.h"
 18 | #include "libavutil/samplefmt.h"
 19 | }
 20 | 
 21 | namespace ros_h264_streamer
 22 | {
 23 | 
 24 | struct H264DecoderImpl
 25 | {
 26 | public:
 27 |   H264DecoderImpl(int width, int height)
 28 |   : m_width(width), m_height(height)
 29 |   {
 30 |     m_stride = 3*m_width;
 31 | 
 32 |     /* For YUV420P -> BGR8 conversion */
 33 |     m_convert_ctx = sws_getContext(m_width, m_height, PIX_FMT_YUV420P, m_width, m_height, PIX_FMT_BGR24, SWS_FAST_BILINEAR, NULL, NULL, NULL);
 34 | 
 35 |     av_init_packet(&avpkt);
 36 | 
 37 |     /* Find H264 codec */
 38 |     avcodec_register_all();
 39 |     codec = avcodec_find_decoder(CODEC_ID_H264);
 40 |     if(!codec)
 41 |     {
 42 |         std::cerr << "[ros_h264_streamer] H264Decoder cannot find H.264 codec in ffmpeg" << std::endl;
 43 |         throw(std::string("H264 codec not found, check that your ffmpeg is built with H264 support"));
 44 |     }
 45 | 
 46 |     c = avcodec_alloc_context3(codec);
 47 |     picture = avcodec_alloc_frame();
 48 |     c->width  = m_width;
 49 |     c->height = m_height;
 50 | 
 51 |     if(avcodec_open2(c, codec, 0) < 0)
 52 |     {
 53 |         std::cerr << "[ros_h264_streamer] H264Decoder cannot open codec" << std::endl;
 54 |         throw(std::string("Cannot open codec"));
 55 |     }
 56 |   }
 57 | 
 58 |   ~H264DecoderImpl()
 59 |   {
 60 |     avcodec_close(c);
 61 |     av_free(c);
 62 |     av_free(picture);
 63 |   }
 64 | 
 65 |   int decode(int frame_size, uint8_t * frame_data, sensor_msgs::ImagePtr & out)
 66 |   {
 67 |     avpkt.size = frame_size;
 68 |     avpkt.data = frame_data;
 69 |     int got_picture;
 70 |     int len = avcodec_decode_video2(c, picture, &got_picture, &avpkt);
 71 |     if(len < 0)
 72 |     {
 73 |         return len;
 74 |     }
 75 |     out->data.resize(m_width*m_height*3);
 76 |     uint8_t *buf_out[4]={&(out->data[0]),NULL,NULL,NULL};
 77 |     sws_scale(m_convert_ctx, (const uint8_t* const*)picture->data, picture->linesize, 0, m_height, buf_out, &m_stride);
 78 | 
 79 |     out->width = m_width;
 80 |     out->height = m_height;
 81 |     out->step = m_stride;
 82 |     out->encoding = sensor_msgs::image_encodings::BGR8;
 83 | 
 84 |     return len;
 85 |   }
 86 | private:
 87 |   int m_width;
 88 |   int m_height;
 89 |   int m_stride;
 90 | 
 91 |   struct SwsContext * m_convert_ctx;
 92 | 
 93 |   AVCodec * codec;
 94 |   AVCodecContext * c;
 95 |   AVFrame *picture;
 96 |   AVPacket avpkt;
 97 | };
 98 | 
 99 | H264Decoder::H264Decoder(int width, int height)
100 | : impl(new H264DecoderImpl(width, height))
101 | {
102 | }
103 | 
104 | int H264Decoder::decode(int frame_size, uint8_t * frame_data, sensor_msgs::ImagePtr & out)
105 | {
106 |   return impl->decode(frame_size, frame_data, out);
107 | }
108 | 
109 | } // namespace ros_h264_streamer
110 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(ros_h264_streamer)
 3 | 
 4 | if(NOT WIN32)
 5 | find_package(catkin REQUIRED COMPONENTS roscpp image_transport cv_bridge)
 6 | include_directories(${catkin_INCLUDE_DIRS})
 7 | else()
 8 | add_definitions("-D_WIN32_WINNT=0x0501")
 9 | include_directories(${CMAKE_INSTALL_PREFIX}/include)
10 | link_directories(${CMAKE_INSTALL_PREFIX}/lib)
11 | set(catkin_LIBRARIES "")
12 | set(CATKIN_PACKAGE_INCLUDE_DESTINATION ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME})
13 | set(CATKIN_PACKAGE_BIN_DESTINATION ${CMAKE_INSTALL_PREFIX}/bin)
14 | set(CATKIN_PACKAGE_LIB_DESTINATION ${CMAKE_INSTALL_PREFIX}/lib)
15 | set(catkin_FOUND 0)
16 | endif()
17 | 
18 | find_package(Boost REQUIRED COMPONENTS program_options)
19 | 
20 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include)
21 | 
22 | if(catkin_FOUND)
23 |   catkin_package(
24 |     INCLUDE_DIRS include
25 |     LIBRARIES ${PROJECT_NAME}
26 |     DEPENDS roscpp image_transport
27 |   )
28 |   find_package(dynamic_graph_bridge_msgs)
29 |     if(${dynamic_graph_bridge_msgs_FOUND})
30 |       include_directories(${dynamic_graph_bridge_msgs_INCLUDE_DIRS})
31 |       add_definitions("-DHAS_DGBRIDGE_MSGS")
32 |     endif()
33 | endif()
34 | 
35 | if(NOT WIN32)
36 | set(ros_h264_streamer_src
37 |   src/h264_encoder.cpp
38 |   src/h264_decoder.cpp
39 |   src/h264_streamer.cpp
40 |   src/h264_receiver.cpp
41 | )
42 | else()
43 | set(ros_h264_streamer_src
44 |   src/h264_encoder.cpp
45 |   src/h264_decoder.cpp
46 |   src/h264_receiver.cpp
47 | )
48 | endif()
49 | 
50 | add_library(ros_h264_streamer SHARED ${ros_h264_streamer_src})
51 | target_link_libraries(ros_h264_streamer ${catkin_LIBRARIES} x264 avcodec avutil swscale)
52 | 
53 | if(NOT WIN32)
54 | # Test encoder/decoder requires opencv, we don't pull this dependency for win32
55 | add_executable(testEncoderDecoder test/testEncoderDecoder.cpp)
56 | target_link_libraries(testEncoderDecoder ros_h264_streamer ${catkin_LIBRARIES})
57 | #Obviously streamer relies way too much on ROS
58 | add_executable(testStreamer test/testStreamer.cpp)
59 | target_link_libraries(testStreamer ros_h264_streamer ${catkin_LIBRARIES} ${Boost_LIBRARIES})
60 | #Idem for mkv_recorder
61 | add_executable(mkv_recorder src/utils/mkv_recorder.cpp src/utils/output/matroska_ebml.c src/utils/output/matroska.c)
62 | target_link_libraries(mkv_recorder ros_h264_streamer ${catkin_LIBRARIES} ${Boost_LIBRARIES})
63 | endif()
64 | 
65 | add_executable(testReceiver test/testReceiver.cpp)
66 | if(NOT WIN32)
67 | target_link_libraries(testReceiver ros_h264_streamer ${catkin_LIBRARIES} ${Boost_LIBRARIES})
68 | else()
69 | target_link_libraries(testReceiver ros_h264_streamer)
70 | endif()
71 | 
72 | install(DIRECTORY include/${PROJECT_NAME}/
73 |   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
74 |   FILES_MATCHING PATTERN "*.h"
75 | )
76 | 
77 | if(NOT WIN32)
78 | set(SHOULD_INSTALL ros_h264_streamer testEncoderDecoder testStreamer testReceiver)
79 | else()
80 | set(SHOULD_INSTALL ros_h264_streamer testReceiver)
81 | endif()
82 | 
83 | install(TARGETS ${SHOULD_INSTALL}
84 |   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
85 |   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
86 |   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
87 | )
88 | 


--------------------------------------------------------------------------------
/src/utils/mkv_recorder.cpp:
--------------------------------------------------------------------------------
  1 | #include <ros/ros.h>
  2 | #include <ros_h264_streamer/h264_encoder.h>
  3 | #include <image_transport/image_transport.h>
  4 | 
  5 | #include <boost/program_options.hpp>
  6 | namespace po = boost::program_options;
  7 | 
  8 | #ifdef HAS_DGBRIDGE_MSGS
  9 | #include <dynamic_graph_bridge_msgs/RunCommand.h>
 10 | #endif
 11 | 
 12 | extern "C"
 13 | {
 14 | #include "x264.h"
 15 | #include "output/output.h"
 16 | }
 17 | 
 18 | struct MKVRecorder
 19 | {
 20 |   MKVRecorder(const std::string & topic, const std::string & camera_name)
 21 |   : nh(), it(nh), topic(topic), camera_name(camera_name),
 22 |     encoder(0), pts(0),
 23 |     hout(0), t0(0)
 24 |   {
 25 |     sub = it.subscribe(topic, 1, &MKVRecorder::imageCallback, this);
 26 |   }
 27 | 
 28 |   ~MKVRecorder()
 29 |   {
 30 |     delete encoder;
 31 |     if(hout)
 32 |     {
 33 |       std::cout << "Closing file" << std::endl;
 34 |       mkv_output.close_file(hout, 0, 0);
 35 |     }
 36 |   }
 37 | 
 38 |   void imageCallback(const sensor_msgs::ImageConstPtr & msg)
 39 |   {
 40 |     if(!encoder)
 41 |     {
 42 |       /* 20 is a high yet effective quality of recording */
 43 |       encoder = new ros_h264_streamer::H264Encoder(msg->width, msg->height, 20, 30, 1, msg->encoding, false);
 44 |       InitMKVRecorder(encoder->GetParameters());
 45 |     }
 46 |     ros_h264_streamer::H264EncoderResult res = encoder->encode(msg, pts);
 47 |     pts = static_cast<x264_picture_t*>(encoder->GetPicIn())->i_pts;
 48 |     pts++;
 49 |     if(res.frame_size)
 50 |     {
 51 |       mkv_output.write_frame(hout, res.frame_data, res.frame_size, static_cast<x264_picture_t*>(encoder->GetPicOut()));
 52 |     }
 53 |   }
 54 | 
 55 |   void spin()
 56 |   {
 57 |     while(ros::ok())
 58 |     {
 59 |       ros::spin();
 60 |     }
 61 |   }
 62 | private:
 63 |   void InitMKVRecorder(x264_param_t * param)
 64 |   {
 65 |     UpdateT0();
 66 |     std::stringstream ss; ss << t0 << "_" << camera_name << ".mkv";
 67 |     cli_output_opt_t output_opt;
 68 |     memset(&output_opt, 0, sizeof(cli_output_opt_t));
 69 | 
 70 |     if(mkv_output.open_file( const_cast<char*>(ss.str().c_str()), &hout, &output_opt))
 71 |     {
 72 |       std::cerr << "Failed to open " << ss.str() << std::endl;
 73 |       throw(std::string("Failed to open output file"));
 74 |     }
 75 | 
 76 |     if(mkv_output.set_param(hout, param))
 77 |     {
 78 |       std::cerr << "Failed to set encoder params on " << ss.str() << std::endl;
 79 |       throw(std::string("Failed to set encoder params"));
 80 |     }
 81 | 
 82 |     x264_nal_t * header;
 83 |     int i_nal;
 84 |     if(x264_encoder_headers(encoder->GetEncoder(), &header, &i_nal) < 0)
 85 |     {
 86 |       std::cerr << "x264_encoder_headers failed" << std::endl;
 87 |       throw(std::string("x264_encoder_headers failed"));
 88 |     }
 89 |     if(mkv_output.write_headers(hout, header) < 0)
 90 |     {
 91 |       std::cerr << "Failed to write headers to " << ss.str() << std::endl;
 92 |       throw(std::string("Failed to write headers"));
 93 |     }
 94 |   }
 95 | 
 96 |   void UpdateT0()
 97 |   {
 98 | #ifdef HAS_DGBRIDGE_MSGS
 99 |     try
100 |     {
101 |       dynamic_graph_bridge_msgs::RunCommand cmd;
102 |       cmd.request.input = "solver.sot.control.time";
103 |       ros::service::call("run_command", cmd);
104 |       std::stringstream ss;
105 |       ss << cmd.response.result;
106 |       ss >> t0;
107 |     }
108 |     catch(...)
109 |     {
110 |       t0 = 0;
111 |     }
112 | #else
113 |     t0 = 0;
114 | #endif
115 |   }
116 | 
117 |   ros::NodeHandle nh;
118 |   image_transport::ImageTransport it;
119 |   std::string topic;
120 |   std::string camera_name;
121 |   image_transport::Subscriber sub;
122 | 
123 |   /* Encoder related */
124 |   ros_h264_streamer::H264Encoder * encoder;
125 |   uint64_t pts;
126 | 
127 |   /* Output */
128 |   hnd_t hout;
129 |   unsigned int t0;
130 | };
131 | 
132 | int main(int argc, char * argv[])
133 | {
134 |   std::string record_topic("/camera/rgb/image_raw");
135 |   std::string camera_name("camera_rgb");
136 |   /* options */
137 |   po::options_description desc("mkv_recorder options");
138 |   desc.add_options()
139 |     ("help", "display help message")
140 |     ("topic,t", po::value<std::string>(&record_topic)->default_value("/camera/rgb/image_raw"), "topic to record")
141 |     ("name,n", po::value<std::string>(&camera_name)->default_value("camera_rgb"), "camera name, the output file will be [record_time]_[camera_name].mkv");
142 | 
143 |   po::variables_map vm;
144 |   po::store(po::parse_command_line(argc, argv, desc), vm);
145 |   po::notify(vm);
146 | 
147 |   if(vm.count("help"))
148 |   {
149 |     std::cout << desc << std::endl;
150 |     return 1;
151 |   }
152 | 
153 |   /* Launch the application */
154 |   std::stringstream ss; ss << camera_name << "_mkv_recorder";
155 |   ros::init(argc, argv, ss.str().c_str());
156 |   MKVRecorder mkv_recorder(record_topic, camera_name);
157 |   mkv_recorder.spin();
158 |   return 0;
159 | }
160 | 


--------------------------------------------------------------------------------
/src/h264_encoder.cpp:
--------------------------------------------------------------------------------
  1 | #include <ros_h264_streamer/h264_encoder.h>
  2 | #include <sensor_msgs/image_encodings.h>
  3 | 
  4 | extern "C"
  5 | {
  6 |   #include "x264.h"
  7 |   #include "libswscale/swscale.h"
  8 | }
  9 | 
 10 | namespace ros_h264_streamer
 11 | {
 12 | 
 13 | struct H264EncoderImpl
 14 | {
 15 | public:
 16 |   H264EncoderImpl(int width, int height, int quality_level, int fps_num, int fps_den, const std::string & encoding, bool streaming)
 17 |   : m_width(width), m_height(height), m_fps_num(fps_num), m_fps_den(fps_den), encoding(encoding)
 18 |   {
 19 |     m_stride = width*3;
 20 | 
 21 |     /* Parametrize x264 for real-time */
 22 |     x264_param_default_preset(&m_param, "veryfast", "animation/zerolatency");
 23 |     m_param.i_threads = 1;
 24 |     m_param.i_width = width;
 25 |     m_param.i_height = height;
 26 |     m_param.i_fps_num = m_fps_num;
 27 |     m_param.i_fps_den = m_fps_den;
 28 |     // Intra refres:
 29 |     m_param.i_keyint_max = m_fps_num/(4*m_fps_den);
 30 |     m_param.i_frame_reference = 1;
 31 |     m_param.b_intra_refresh = 1;
 32 |     //Rate control:
 33 |     m_param.rc.i_rc_method = X264_RC_CRF;
 34 |     m_param.rc.f_rf_constant = quality_level;
 35 |     m_param.rc.f_rf_constant_max = 100;
 36 | 
 37 |     if(streaming)
 38 |     {
 39 |       //For streaming:
 40 |       m_param.b_repeat_headers = 1;
 41 |       m_param.b_annexb = 1;
 42 |     }
 43 |     else
 44 |     {
 45 |       // Specific for video encoding
 46 |       m_param.b_vfr_input = 1;
 47 |       m_param.i_timebase_num = m_fps_den;
 48 |       m_param.i_timebase_den = m_fps_num;
 49 |       m_param.vui.i_sar_width  = 1;
 50 |       m_param.vui.i_sar_height = 1;
 51 |       m_param.i_frame_total = 0;
 52 | 
 53 |       m_param.b_intra_refresh = 0;
 54 |       m_param.b_repeat_headers = 0;
 55 |       m_param.b_annexb = 0;
 56 |       m_param.i_csp = X264_CSP_I420;
 57 |     }
 58 | 
 59 |     Init(&m_param);
 60 |   }
 61 | 
 62 |   ~H264EncoderImpl()
 63 |   {
 64 |     sws_freeContext(m_convert_ctx);
 65 |     x264_encoder_close(m_encoder);
 66 |   }
 67 | 
 68 |   H264EncoderResult encode(const sensor_msgs::ImageConstPtr & img, uint64_t pts)
 69 |   {
 70 |     x264_nal_t* nals;
 71 |     int i_nals;
 72 |     H264EncoderResult res;
 73 | 
 74 |     /* Convert from image encoding to YUV420P */
 75 |     uint8_t *buf_in[4]={(uint8_t*)(&(img->data[0])),NULL,NULL,NULL};
 76 |     sws_scale(m_convert_ctx, (const uint8_t* const*)buf_in, &m_stride, 0, m_height, m_pic_in.img.plane, m_pic_in.img.i_stride);
 77 |     m_pic_in.i_pts = pts;
 78 | 
 79 |     /* Encode */
 80 |     while( (res.frame_size = x264_encoder_encode(m_encoder, &nals, &i_nals, &m_pic_in, &m_pic_out)) == 0 ) { m_pic_in.i_pts++; }
 81 |     res.frame_data = nals[0].p_payload;
 82 | 
 83 |     return res;
 84 |   }
 85 | 
 86 |   x264_param_t * GetParameters()
 87 |   {
 88 |     return &m_param;
 89 |   }
 90 | 
 91 |   x264_t * GetEncoder()
 92 |   {
 93 |     return m_encoder;
 94 |   }
 95 | 
 96 |   void * GetPicIn()
 97 |   {
 98 |     return &m_pic_in;
 99 |   }
100 | 
101 |   void * GetPicOut()
102 |   {
103 |     return &m_pic_out;
104 |   }
105 | private:
106 |   void Init(x264_param_t * param)
107 |   {
108 |     m_encoder = x264_encoder_open(param);
109 |     m_width = param->i_width;
110 |     m_height = param->i_height;
111 |     m_stride = m_width*3;
112 |     x264_picture_alloc(&m_pic_in, X264_CSP_I420, m_width, m_height);
113 |     if(encoding == sensor_msgs::image_encodings::RGB8)
114 |     {
115 |       m_convert_ctx = sws_getContext(m_width, m_height, PIX_FMT_RGB24, m_width, m_height, PIX_FMT_YUV420P, SWS_FAST_BILINEAR, NULL, NULL, NULL);
116 |     }
117 |     else if(encoding == sensor_msgs::image_encodings::BGR8)
118 |     {
119 |       m_convert_ctx = sws_getContext(m_width, m_height, PIX_FMT_BGR24, m_width, m_height, PIX_FMT_YUV420P, SWS_FAST_BILINEAR, NULL, NULL, NULL);
120 |     }
121 |     else if(encoding == sensor_msgs::image_encodings::RGBA8)
122 |     {
123 |       m_stride = m_width*4;
124 |       m_convert_ctx = sws_getContext(m_width, m_height, PIX_FMT_RGBA, m_width, m_height, PIX_FMT_YUV420P, SWS_FAST_BILINEAR, NULL, NULL, NULL);
125 |     }
126 |     else if(encoding == sensor_msgs::image_encodings::BGRA8)
127 |     {
128 |       m_stride = m_width*4;
129 |       m_convert_ctx = sws_getContext(m_width, m_height, PIX_FMT_BGRA, m_width, m_height, PIX_FMT_YUV420P, SWS_FAST_BILINEAR, NULL, NULL, NULL);
130 |     }
131 |     else
132 |     {
133 |       std::cerr << "[ros_h264_streamer] Trying to work with unsupported encoding!" << std::endl;
134 |     }
135 |   }
136 | 
137 |   int m_width;
138 |   int m_height;
139 |   int m_fps_num;
140 |   int m_fps_den;
141 |   std::string encoding;
142 |   int m_stride;
143 | 
144 |   struct SwsContext * m_convert_ctx;
145 | 
146 |   x264_param_t m_param;
147 |   x264_t * m_encoder;
148 |   x264_picture_t m_pic_in, m_pic_out;
149 | };
150 | 
151 | H264Encoder::H264Encoder(int width, int height, int quality_level, int fps_num, int fps_den, const std::string & encoding, bool streaming)
152 | : impl(new H264EncoderImpl(width, height, quality_level, fps_num, fps_den, encoding, streaming))
153 | {
154 | }
155 | 
156 | H264EncoderResult H264Encoder::encode(const sensor_msgs::ImageConstPtr & img, uint64_t pts)
157 | {
158 |   return impl->encode(img, pts);
159 | }
160 | 
161 | x264_param_t * H264Encoder::GetParameters()
162 | {
163 |   return impl->GetParameters();
164 | }
165 | 
166 | x264_t * H264Encoder::GetEncoder()
167 | {
168 |   return impl->GetEncoder();
169 | }
170 | 
171 | void * H264Encoder::GetPicIn()
172 | {
173 |   return impl->GetPicIn();
174 | }
175 | 
176 | void * H264Encoder::GetPicOut()
177 | {
178 |   return impl->GetPicOut();
179 | }
180 | 
181 | } // namespace ros_h264_streamer
182 | 


--------------------------------------------------------------------------------
/src/utils/output/matroska.c:
--------------------------------------------------------------------------------
  1 | /*****************************************************************************
  2 |  * matroska.c: matroska muxer
  3 |  *****************************************************************************
  4 |  * Copyright (C) 2005-2012 x264 project
  5 |  *
  6 |  * Authors: Mike Matsnev <mike@haali.su>
  7 |  *
  8 |  * This program is free software; you can redistribute it and/or modify
  9 |  * it under the terms of the GNU General Public License as published by
 10 |  * the Free Software Foundation; either version 2 of the License, or
 11 |  * (at your option) any later version.
 12 |  *
 13 |  * This program is distributed in the hope that it will be useful,
 14 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 15 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 16 |  * GNU General Public License for more details.
 17 |  *
 18 |  * You should have received a copy of the GNU General Public License
 19 |  * along with this program; if not, write to the Free Software
 20 |  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02111, USA.
 21 |  *
 22 |  * This program is also available under a commercial proprietary license.
 23 |  * For more information, contact us at licensing@x264.com.
 24 |  *****************************************************************************/
 25 | 
 26 | #include "output.h"
 27 | #include <string.h>
 28 | #include "matroska_ebml.h"
 29 | 
 30 | typedef struct
 31 | {
 32 |     mk_writer *w;
 33 | 
 34 |     int width, height, d_width, d_height;
 35 | 
 36 |     int display_size_units;
 37 | 
 38 |     int64_t frame_duration;
 39 | 
 40 |     char b_writing_frame;
 41 |     uint32_t i_timebase_num;
 42 |     uint32_t i_timebase_den;
 43 | 
 44 | } mkv_hnd_t;
 45 | 
 46 | static int open_file( char *psz_filename, hnd_t *p_handle, cli_output_opt_t *opt )
 47 | {
 48 |     mkv_hnd_t *p_mkv;
 49 | 
 50 |     *p_handle = NULL;
 51 | 
 52 |     p_mkv  = malloc( sizeof(*p_mkv) );
 53 |     if( !p_mkv )
 54 |         return -1;
 55 | 
 56 |     memset( p_mkv, 0, sizeof(*p_mkv) );
 57 | 
 58 |     p_mkv->w = mk_create_writer( psz_filename );
 59 |     if( !p_mkv->w )
 60 |     {
 61 |         free( p_mkv );
 62 |         return -1;
 63 |     }
 64 | 
 65 |     *p_handle = p_mkv;
 66 | 
 67 |     return 0;
 68 | }
 69 | 
 70 | static int set_param( hnd_t handle, x264_param_t *p_param )
 71 | {
 72 |     mkv_hnd_t   *p_mkv = handle;
 73 |     int64_t dw, dh;
 74 | 
 75 |     if( p_param->i_fps_num > 0 && !p_param->b_vfr_input )
 76 |     {
 77 |         p_mkv->frame_duration = (int64_t)p_param->i_fps_den *
 78 |                                 (int64_t)1000000000 / p_param->i_fps_num;
 79 |     }
 80 |     else
 81 |     {
 82 |         p_mkv->frame_duration = 0;
 83 |     }
 84 | 
 85 |     p_mkv->width = p_mkv->d_width = p_param->i_width;
 86 |     p_mkv->height = p_mkv->d_height = p_param->i_height;
 87 |     p_mkv->display_size_units = DS_PIXELS;
 88 | 
 89 |     if( p_param->vui.i_sar_width && p_param->vui.i_sar_height
 90 |         && p_param->vui.i_sar_width != p_param->vui.i_sar_height )
 91 |     {
 92 |         if ( p_param->vui.i_sar_width > p_param->vui.i_sar_height ) {
 93 |             dw = (int64_t)p_param->i_width * p_param->vui.i_sar_width / p_param->vui.i_sar_height;
 94 |             dh = p_param->i_height;
 95 |         } else {
 96 |             dw = p_param->i_width;
 97 |             dh = (int64_t)p_param->i_height * p_param->vui.i_sar_height / p_param->vui.i_sar_width;
 98 |         }
 99 | 
100 |         p_mkv->d_width = (int)dw;
101 |         p_mkv->d_height = (int)dh;
102 |     }
103 | 
104 |     p_mkv->i_timebase_num = p_param->i_timebase_num;
105 |     p_mkv->i_timebase_den = p_param->i_timebase_den;
106 | 
107 |     return 0;
108 | }
109 | 
110 | static int write_headers( hnd_t handle, x264_nal_t *p_nal )
111 | {
112 |     mkv_hnd_t *p_mkv = handle;
113 | 
114 |     int sps_size = p_nal[0].i_payload - 4;
115 |     int pps_size = p_nal[1].i_payload - 4;
116 |     int sei_size = p_nal[2].i_payload;
117 | 
118 |     uint8_t *sps = p_nal[0].p_payload + 4;
119 |     uint8_t *pps = p_nal[1].p_payload + 4;
120 |     uint8_t *sei = p_nal[2].p_payload;
121 | 
122 |     int ret;
123 |     uint8_t *avcC;
124 |     int avcC_len;
125 | 
126 |     if( !p_mkv->width || !p_mkv->height ||
127 |         !p_mkv->d_width || !p_mkv->d_height )
128 |         return -1;
129 | 
130 |     avcC_len = 5 + 1 + 2 + sps_size + 1 + 2 + pps_size;
131 |     avcC = malloc( avcC_len );
132 |     if( !avcC )
133 |         return -1;
134 | 
135 |     avcC[0] = 1;
136 |     avcC[1] = sps[1];
137 |     avcC[2] = sps[2];
138 |     avcC[3] = sps[3];
139 |     avcC[4] = 0xff; // nalu size length is four bytes
140 |     avcC[5] = 0xe1; // one sps
141 | 
142 |     avcC[6] = sps_size >> 8;
143 |     avcC[7] = sps_size;
144 | 
145 |     memcpy( avcC+8, sps, sps_size );
146 | 
147 |     avcC[8+sps_size] = 1; // one pps
148 |     avcC[9+sps_size] = pps_size >> 8;
149 |     avcC[10+sps_size] = pps_size;
150 | 
151 |     memcpy( avcC+11+sps_size, pps, pps_size );
152 | 
153 |     ret = mk_write_header( p_mkv->w, "x264" X264_VERSION, "V_MPEG4/ISO/AVC",
154 |                            avcC, avcC_len, p_mkv->frame_duration, 50000,
155 |                            p_mkv->width, p_mkv->height,
156 |                            p_mkv->d_width, p_mkv->d_height, p_mkv->display_size_units );
157 |     if( ret < 0 )
158 |         return ret;
159 | 
160 |     free( avcC );
161 | 
162 |     // SEI
163 | 
164 |     if( !p_mkv->b_writing_frame )
165 |     {
166 |         if( mk_start_frame( p_mkv->w ) < 0 )
167 |             return -1;
168 |         p_mkv->b_writing_frame = 1;
169 |     }
170 |     if( mk_add_frame_data( p_mkv->w, sei, sei_size ) < 0 )
171 |         return -1;
172 | 
173 |     return sei_size + sps_size + pps_size;
174 | }
175 | 
176 | static int write_frame( hnd_t handle, uint8_t *p_nalu, int i_size, x264_picture_t *p_picture )
177 | {
178 |     mkv_hnd_t *p_mkv = handle;
179 | 
180 |     if( !p_mkv->b_writing_frame )
181 |     {
182 |         if( mk_start_frame( p_mkv->w ) < 0 )
183 |             return -1;
184 |         p_mkv->b_writing_frame = 1;
185 |     }
186 | 
187 |     if( mk_add_frame_data( p_mkv->w, p_nalu, i_size ) < 0 )
188 |         return -1;
189 | 
190 |     int64_t i_stamp = (int64_t)((p_picture->i_pts * 1e9 * p_mkv->i_timebase_num / p_mkv->i_timebase_den) + 0.5);
191 | 
192 |     p_mkv->b_writing_frame = 0;
193 | 
194 |     if( mk_set_frame_flags( p_mkv->w, i_stamp, p_picture->b_keyframe, p_picture->i_type == X264_TYPE_B ) < 0 )
195 |         return -1;
196 | 
197 |     return i_size;
198 | }
199 | 
200 | static int close_file( hnd_t handle, int64_t largest_pts, int64_t second_largest_pts )
201 | {
202 |     mkv_hnd_t *p_mkv = handle;
203 |     int ret;
204 |     int64_t i_last_delta;
205 | 
206 |     i_last_delta = p_mkv->i_timebase_den ? (int64_t)(((largest_pts - second_largest_pts) * p_mkv->i_timebase_num / p_mkv->i_timebase_den) + 0.5) : 0;
207 | 
208 |     ret = mk_close( p_mkv->w, i_last_delta );
209 | 
210 |     free( p_mkv );
211 | 
212 |     return ret;
213 | }
214 | 
215 | const cli_output_t mkv_output = { open_file, set_param, write_headers, write_frame, close_file };
216 | 


--------------------------------------------------------------------------------
/src/h264_streamer.cpp:
--------------------------------------------------------------------------------
  1 | #include <boost/asio.hpp>
  2 | #include <boost/thread.hpp>
  3 | 
  4 | #include <ros_h264_streamer/h264_streamer.h>
  5 | 
  6 | #include <ros/ros.h>
  7 | 
  8 | #include <ros_h264_streamer/h264_encoder.h>
  9 | #include <image_transport/image_transport.h>
 10 | 
 11 | #include "private/net_buffer_size.h"
 12 | 
 13 | using boost::asio::ip::udp;
 14 | using boost::asio::ip::tcp;
 15 | 
 16 | namespace ros_h264_streamer
 17 | {
 18 | 
 19 | enum H264StreamerProtocol
 20 | {
 21 |   ChunkIDPlusData = 1,
 22 |   FrameSizePlusData
 23 | };
 24 | 
 25 | struct H264StreamerNetImpl
 26 | {
 27 |   H264StreamerNetImpl()
 28 |   : io_service(), io_service_th(0), request_data(0), chunk_data_size(0), chunk_data(0), protocol(ChunkIDPlusData)
 29 |   {
 30 |   }
 31 | 
 32 |   void InitBuffers()
 33 |   {
 34 |     SetChunkDataSize();
 35 |     request_data = new char[ros_h264_streamer_private::_request_size];
 36 |     chunk_data = new unsigned char[chunk_data_size];
 37 |     CleanRequestData();
 38 |     CleanChunkData();
 39 |   }
 40 | 
 41 |   virtual void StartNetworking()
 42 |   {
 43 |   }
 44 | 
 45 |   ~H264StreamerNetImpl()
 46 |   {
 47 |     io_service.stop();
 48 |     if(io_service_th)
 49 |     {
 50 |       io_service_th->join();
 51 |       delete io_service_th;
 52 |     }
 53 |     delete[] request_data;
 54 |     delete[] chunk_data;
 55 |   }
 56 | 
 57 |   virtual void SetChunkDataSize() = 0;
 58 | 
 59 |   void IOServiceThread()
 60 |   {
 61 |     while(ros::ok())
 62 |     {
 63 |       io_service.run();
 64 |       io_service.reset();
 65 |     }
 66 |   }
 67 | 
 68 |   void StartIOService()
 69 |   {
 70 |     io_service_th = new boost::thread(boost::bind(&H264StreamerNetImpl::IOServiceThread, this));
 71 |   }
 72 | 
 73 |   void HandleNewData(H264EncoderResult & res)
 74 |   {
 75 |     if(protocol == ChunkIDPlusData)
 76 |     {
 77 |       int data_size = 0;
 78 |       uint8_t chunkID = 0;
 79 |       do
 80 |       {
 81 |         CleanChunkData();
 82 |         data_size = std::min(res.frame_size, chunk_data_size - 1 );
 83 |         chunk_data[0] = chunkID;
 84 |         std::memcpy(&chunk_data[1], &res.frame_data[chunkID*(chunk_data_size - 1)], data_size);
 85 |         SendData(data_size + 1);
 86 |         chunkID++;
 87 |         res.frame_size -= data_size;
 88 |       }
 89 |       while(res.frame_size > 0);
 90 |     }
 91 |     else
 92 |     {
 93 |       if(chunk_data_size < res.frame_size + sizeof(unsigned int))
 94 |       {
 95 |         delete[] chunk_data;
 96 |         chunk_data_size = 2*res.frame_size + sizeof(int);
 97 |         chunk_data = new uint8_t[chunk_data_size];
 98 |       }
 99 |       CleanChunkData();
100 |       std::memcpy(chunk_data, &res.frame_size, sizeof(int));
101 |       std::memcpy(&chunk_data[sizeof(int)], res.frame_data, res.frame_size);
102 |       SendData(res.frame_size + sizeof(int));
103 |     }
104 |   }
105 | 
106 |   virtual void SendData(int frame_size) = 0;
107 | 
108 |   boost::asio::io_service io_service;
109 |   boost::thread * io_service_th;
110 | 
111 |   char * request_data;
112 |   void CleanRequestData() { memset(request_data, 0, ros_h264_streamer_private::_request_size); }
113 |   int chunk_data_size;
114 |   unsigned char * chunk_data;
115 |   void CleanChunkData() { memset(chunk_data, 0, chunk_data_size); }
116 | 
117 |   H264StreamerProtocol protocol;
118 | };
119 | 
120 | struct H264StreamerUDPServer : public H264StreamerNetImpl
121 | {
122 |   H264StreamerUDPServer(short port)
123 |   : socket(0), has_client(false), client_endpoint()
124 |   {
125 |     socket = new udp::socket(io_service, udp::endpoint(udp::v4(), port));
126 |   }
127 | 
128 |   ~H264StreamerUDPServer()
129 |   {
130 |     socket->close();
131 |     delete socket;
132 |   }
133 | 
134 |   virtual void StartNetworking()
135 |   {
136 |     ReceiveData();
137 |   }
138 | 
139 |   void SetChunkDataSize()
140 |   {
141 |     chunk_data_size = ros_h264_streamer_private::_udp_video_chunk_size;
142 |   }
143 | 
144 |   void SendData(int frame_size)
145 |   {
146 |     if(has_client)
147 |     {
148 |       boost::system::error_code error;
149 |       socket->send_to(
150 |         boost::asio::buffer(chunk_data, frame_size),
151 |         client_endpoint, 0, error);
152 |       if(error)
153 |       {
154 |         std::cerr << "[ros_h264_streamer] H264Streamer UDP server got the error while sending data: " << std::endl << error.message() << std::endl;
155 |         has_client = false;
156 |       }
157 |     }
158 |   }
159 | 
160 |   void ReceiveData()
161 |   {
162 |     socket->async_receive_from(
163 |       boost::asio::buffer(request_data, ros_h264_streamer_private::_request_size), request_endpoint,
164 |       boost::bind(&H264StreamerUDPServer::handle_receive_from, this,
165 |         boost::asio::placeholders::error,
166 |         boost::asio::placeholders::bytes_transferred));
167 |   }
168 | 
169 |   void handle_receive_from(const boost::system::error_code & error, size_t bytes_recvd)
170 |   {
171 |     if(!error && bytes_recvd > 0)
172 |     {
173 |       has_client = true;
174 |       client_endpoint = request_endpoint;
175 |     }
176 |     else if(error)
177 |     {
178 |       std::cerr << "[ros_h264_streamer] H264Streamer UDP server got the error while receiving data: " << std::endl << error.message() << std::endl;
179 |     }
180 |     ReceiveData();
181 |   }
182 | 
183 | private:
184 |   udp::socket * socket;
185 |   bool has_client;
186 |   udp::endpoint request_endpoint;
187 |   udp::endpoint client_endpoint;
188 | };
189 | 
190 | struct H264StreamerUDPClient : public H264StreamerNetImpl
191 | {
192 |   H264StreamerUDPClient(const std::string & host, short port)
193 |   {
194 |     udp::resolver resolver(io_service);
195 |     std::stringstream ss;
196 |     ss << port;
197 |     udp::resolver::query query(udp::v4(), host, ss.str());
198 |     server_endpoint = *resolver.resolve(query);
199 | 
200 |     socket = new udp::socket(io_service);
201 |     socket->open(udp::v4());
202 |   }
203 | 
204 |   void SetChunkDataSize()
205 |   {
206 |     chunk_data_size = ros_h264_streamer_private::_udp_video_chunk_size;
207 |   }
208 | 
209 |   void SendData(int frame_size)
210 |   {
211 |     boost::system::error_code error;
212 |     socket->send_to(
213 |       boost::asio::buffer(chunk_data, frame_size),
214 |       server_endpoint, 0, error);
215 |     if(error)
216 |     {
217 |       std::cerr << "[ros_h264_streamer] H264Streamer UDP client got the error while sending data: " << std::endl << error.message() << std::endl;
218 |     }
219 |   }
220 | private:
221 |   udp::socket * socket;
222 |   udp::endpoint server_endpoint;
223 | };
224 | 
225 | struct H264StreamerTCPServer : public H264StreamerNetImpl
226 | {
227 |   H264StreamerTCPServer(short port)
228 |   : acceptor(io_service, tcp::endpoint(tcp::v4(), port)),
229 |     socket(0)
230 |   {
231 |     AcceptConnection();
232 |   }
233 | 
234 |   ~H264StreamerTCPServer()
235 |   {
236 |     acceptor.close();
237 |     if(socket)
238 |     {
239 |       socket->close();
240 |     }
241 |     delete socket;
242 |   }
243 | 
244 |   void SetChunkDataSize()
245 |   {
246 |     chunk_data_size = ros_h264_streamer_private::_tcp_video_chunk_size;
247 |   }
248 | 
249 |   void SendData(int frame_size)
250 |   {
251 |     if(socket)
252 |     {
253 |       socket->async_send(
254 |         boost::asio::buffer(chunk_data, frame_size),
255 |         boost::bind(&H264StreamerTCPServer::handle_send, this,
256 |           boost::asio::placeholders::error,
257 |           boost::asio::placeholders::bytes_transferred));
258 |     }
259 |   }
260 | 
261 |   void AcceptConnection()
262 |   {
263 |     tcp::socket * nsocket = new tcp::socket(io_service);
264 |     acceptor.async_accept(*nsocket,
265 |       boost::bind(&H264StreamerTCPServer::handle_accept, this, nsocket, boost::asio::placeholders::error));
266 |   }
267 | 
268 |   void handle_accept(tcp::socket * socket_in, const boost::system::error_code& error)
269 |   {
270 |     delete socket;
271 |     if(!error)
272 |     {
273 |       socket = socket_in;
274 |       boost::asio::ip::tcp::no_delay option(true);
275 |       socket->set_option(option);
276 |     }
277 |     else
278 |     {
279 |       socket = 0;
280 |     }
281 |     AcceptConnection();
282 |   }
283 | 
284 |   void handle_send(const boost::system::error_code & error, size_t bytes_send)
285 |   {
286 |     if(error)
287 |     {
288 |       if(socket)
289 |       {
290 |         socket->cancel();
291 |         socket->close();
292 |       }
293 |       delete socket;
294 |       socket = 0;
295 |     }
296 |   }
297 | private:
298 |   tcp::acceptor acceptor;
299 |   tcp::socket * socket;
300 | };
301 | 
302 | struct H264StreamerTCPClient : public H264StreamerNetImpl
303 | {
304 |   H264StreamerTCPClient(const std::string & host, short port)
305 |   : socket(0), server_endpoint(), ready(false)
306 |   {
307 |     tcp::resolver resolver(io_service);
308 |     std::stringstream ss;
309 |     ss << port;
310 |     tcp::resolver::query query(tcp::v4(), host, ss.str());
311 |     server_endpoint = *resolver.resolve(query);
312 | 
313 |     ConnectToServer();
314 |   }
315 | 
316 |   void SetChunkDataSize()
317 |   {
318 |     chunk_data_size = ros_h264_streamer_private::_tcp_video_chunk_size;
319 |   }
320 | 
321 |   void ConnectToServer()
322 |   {
323 |     delete socket;
324 |     socket = new tcp::socket(io_service);
325 |     socket->async_connect(server_endpoint,
326 |       boost::bind(&H264StreamerTCPClient::handle_connect, this,
327 |         boost::asio::placeholders::error));
328 |   }
329 | 
330 |   void SendData(int frame_size)
331 |   {
332 |     if(ready)
333 |     {
334 |       socket->async_send(
335 |         boost::asio::buffer(chunk_data, frame_size), 0,
336 |         boost::bind(&H264StreamerTCPClient::handle_send, this,
337 |           boost::asio::placeholders::error,
338 |           boost::asio::placeholders::bytes_transferred));
339 |     }
340 |   }
341 | 
342 |   void handle_connect(const boost::system::error_code & error)
343 |   {
344 |     if(!error)
345 |     {
346 |       boost::asio::ip::tcp::no_delay option(true);
347 |       socket->set_option(option);
348 |       ready = true;
349 |     }
350 |     else
351 |     {
352 |       ready = false;
353 |       sleep(1);
354 |       ConnectToServer();
355 |     }
356 |   }
357 | 
358 |   void handle_send(const boost::system::error_code & error, size_t bytes_send)
359 |   {
360 |     if(error)
361 |     {
362 |       ready = false;
363 |       std::cerr << "[ros_h264_streamer] H264Streamer TCP client got error when sending: " << std::endl << error.message() << std::endl;
364 |       std::cerr << "[ros_h264_streamer] Trying to reconnect" << std::endl;
365 |       ConnectToServer();
366 |     }
367 |   }
368 | private:
369 |   tcp::socket * socket;
370 |   tcp::endpoint server_endpoint;
371 |   bool ready;
372 | };
373 | 
374 | struct H264StreamerImpl
375 | {
376 | public:
377 |   H264StreamerImpl(H264Streamer::Config & conf, ros::NodeHandle & nh)
378 |   : nh(nh), it(nh), conf(conf), net_impl(0), encoder(0), skip_frame(0)
379 |   {
380 |     if(conf.udp)
381 |     {
382 |       if(conf.server)
383 |       {
384 |         net_impl = new H264StreamerUDPServer(conf.port);
385 |       }
386 |       else
387 |       {
388 |         net_impl = new H264StreamerUDPClient(conf.host, conf.port);
389 |       }
390 |       net_impl->protocol = ChunkIDPlusData;
391 |     }
392 |     else
393 |     {
394 |       if(conf.server)
395 |       {
396 |         net_impl = new H264StreamerTCPServer(conf.port);
397 |       }
398 |       else
399 |       {
400 |         net_impl = new H264StreamerTCPClient(conf.host, conf.port);
401 |       }
402 |       net_impl->protocol = ChunkIDPlusData;
403 |     }
404 |   }
405 | 
406 |   void Init()
407 |   {
408 |     net_impl->InitBuffers();
409 |     net_impl->StartIOService();
410 |     net_impl->StartNetworking();
411 |     sub = it.subscribe(conf.camera_topic, 1, &H264StreamerImpl::imageCallback, this);
412 |   }
413 | 
414 |   ~H264StreamerImpl()
415 |   {
416 |     sub.shutdown();
417 |     delete encoder;
418 |     delete net_impl;
419 |   }
420 | 
421 |   void imageCallback(const sensor_msgs::ImageConstPtr & msg)
422 |   {
423 |     if(!encoder)
424 |     {
425 |       encoder = new H264Encoder(msg->width, msg->height, conf.quality, conf.fps_num, conf.fps_den, msg->encoding);
426 |     }
427 |     if(skip_frame == 0)
428 |     {
429 |       H264EncoderResult res = encoder->encode(msg);
430 |       net_impl->HandleNewData(res);
431 |     }
432 |     skip_frame++;
433 |     if(skip_frame == conf.fps_den)
434 |     {
435 |       skip_frame = 0;
436 |     }
437 |   }
438 | private:
439 |   ros::NodeHandle & nh;
440 |   image_transport::ImageTransport it;
441 |   image_transport::Subscriber sub;
442 |   H264Streamer::Config & conf;
443 | 
444 |   H264StreamerNetImpl * net_impl;
445 | 
446 |   H264Encoder * encoder;
447 |   int skip_frame;
448 | };
449 | 
450 | H264Streamer::H264Streamer(H264Streamer::Config & conf, ros::NodeHandle & nh)
451 | : impl(new H264StreamerImpl(conf, nh))
452 | {
453 |   impl->Init();
454 | }
455 | 
456 | } // namespace ros_h264_streamer
457 | 


--------------------------------------------------------------------------------
/src/utils/output/matroska_ebml.c:
--------------------------------------------------------------------------------
  1 | /*****************************************************************************
  2 |  * matroska_ebml.c: matroska muxer utilities
  3 |  *****************************************************************************
  4 |  * Copyright (C) 2005-2012 x264 project
  5 |  *
  6 |  * Authors: Mike Matsnev <mike@haali.su>
  7 |  *
  8 |  * This program is free software; you can redistribute it and/or modify
  9 |  * it under the terms of the GNU General Public License as published by
 10 |  * the Free Software Foundation; either version 2 of the License, or
 11 |  * (at your option) any later version.
 12 |  *
 13 |  * This program is distributed in the hope that it will be useful,
 14 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 15 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 16 |  * GNU General Public License for more details.
 17 |  *
 18 |  * You should have received a copy of the GNU General Public License
 19 |  * along with this program; if not, write to the Free Software
 20 |  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02111, USA.
 21 |  *
 22 |  * This program is also available under a commercial proprietary license.
 23 |  * For more information, contact us at licensing@x264.com.
 24 |  *****************************************************************************/
 25 | 
 26 | #include "output.h"
 27 | #include "matroska_ebml.h"
 28 | 
 29 | #include <stdio.h>
 30 | #include <string.h>
 31 | #include <sys/stat.h>
 32 | 
 33 | static inline uint8_t x264_is_regular_file( FILE *filehandle )
 34 | {   
 35 |     struct stat file_stat;
 36 |     if( fstat( fileno( filehandle ), &file_stat ) )
 37 |         return -1;
 38 |     return S_ISREG( file_stat.st_mode );
 39 | }
 40 | 
 41 | #define CLSIZE 1048576
 42 | #define CHECK(x)\
 43 | do {\
 44 |     if( (x) < 0 )\
 45 |         return -1;\
 46 | } while( 0 )
 47 | 
 48 | struct mk_context
 49 | {
 50 |     struct mk_context *next, **prev, *parent;
 51 |     mk_writer *owner;
 52 |     unsigned id;
 53 | 
 54 |     void *data;
 55 |     unsigned d_cur, d_max;
 56 | };
 57 | 
 58 | typedef struct mk_context mk_context;
 59 | 
 60 | struct mk_writer
 61 | {
 62 |     FILE *fp;
 63 | 
 64 |     unsigned duration_ptr;
 65 | 
 66 |     mk_context *root, *cluster, *frame;
 67 |     mk_context *freelist;
 68 |     mk_context *actlist;
 69 | 
 70 |     int64_t def_duration;
 71 |     int64_t timescale;
 72 |     int64_t cluster_tc_scaled;
 73 |     int64_t frame_tc, max_frame_tc;
 74 | 
 75 |     char wrote_header, in_frame, keyframe, skippable;
 76 | };
 77 | 
 78 | static mk_context *mk_create_context( mk_writer *w, mk_context *parent, unsigned id )
 79 | {
 80 |     mk_context *c;
 81 | 
 82 |     if( w->freelist )
 83 |     {
 84 |         c = w->freelist;
 85 |         w->freelist = w->freelist->next;
 86 |     }
 87 |     else
 88 |     {
 89 |         c = malloc( sizeof(*c) );
 90 |         if( !c )
 91 |             return NULL;
 92 |         memset( c, 0, sizeof(*c) );
 93 |     }
 94 | 
 95 |     c->parent = parent;
 96 |     c->owner = w;
 97 |     c->id = id;
 98 | 
 99 |     if( c->owner->actlist )
100 |         c->owner->actlist->prev = &c->next;
101 |     c->next = c->owner->actlist;
102 |     c->prev = &c->owner->actlist;
103 |     c->owner->actlist = c;
104 | 
105 |     return c;
106 | }
107 | 
108 | static int mk_append_context_data( mk_context *c, const void *data, unsigned size )
109 | {
110 |     unsigned ns = c->d_cur + size;
111 | 
112 |     if( ns > c->d_max )
113 |     {
114 |         void *dp;
115 |         unsigned dn = c->d_max ? c->d_max << 1 : 16;
116 |         while( ns > dn )
117 |             dn <<= 1;
118 | 
119 |         dp = realloc( c->data, dn );
120 |         if( !dp )
121 |             return -1;
122 | 
123 |         c->data = dp;
124 |         c->d_max = dn;
125 |     }
126 | 
127 |     memcpy( (char*)c->data + c->d_cur, data, size );
128 | 
129 |     c->d_cur = ns;
130 | 
131 |     return 0;
132 | }
133 | 
134 | static int mk_write_id( mk_context *c, unsigned id )
135 | {
136 |     unsigned char c_id[4] = { id >> 24, id >> 16, id >> 8, id };
137 | 
138 |     if( c_id[0] )
139 |         return mk_append_context_data( c, c_id, 4 );
140 |     if( c_id[1] )
141 |         return mk_append_context_data( c, c_id+1, 3 );
142 |     if( c_id[2] )
143 |         return mk_append_context_data( c, c_id+2, 2 );
144 |     return mk_append_context_data( c, c_id+3, 1 );
145 | }
146 | 
147 | static int mk_write_size( mk_context *c, unsigned size )
148 | {
149 |     unsigned char c_size[5] = { 0x08, size >> 24, size >> 16, size >> 8, size };
150 | 
151 |     if( size < 0x7f )
152 |     {
153 |         c_size[4] |= 0x80;
154 |         return mk_append_context_data( c, c_size+4, 1 );
155 |     }
156 |     if( size < 0x3fff )
157 |     {
158 |         c_size[3] |= 0x40;
159 |         return mk_append_context_data( c, c_size+3, 2 );
160 |     }
161 |     if( size < 0x1fffff )
162 |     {
163 |         c_size[2] |= 0x20;
164 |         return mk_append_context_data( c, c_size+2, 3 );
165 |     }
166 |     if( size < 0x0fffffff )
167 |     {
168 |         c_size[1] |= 0x10;
169 |         return mk_append_context_data( c, c_size+1, 4 );
170 |     }
171 |     return mk_append_context_data( c, c_size, 5 );
172 | }
173 | 
174 | static int mk_flush_context_id( mk_context *c )
175 | {
176 |     unsigned char ff = 0xff;
177 | 
178 |     if( !c->id )
179 |         return 0;
180 | 
181 |     CHECK( mk_write_id( c->parent, c->id ) );
182 |     CHECK( mk_append_context_data( c->parent, &ff, 1 ) );
183 | 
184 |     c->id = 0;
185 | 
186 |     return 0;
187 | }
188 | 
189 | static int mk_flush_context_data( mk_context *c )
190 | {
191 |     if( !c->d_cur )
192 |         return 0;
193 | 
194 |     if( c->parent )
195 |         CHECK( mk_append_context_data( c->parent, c->data, c->d_cur ) );
196 |     else if( fwrite( c->data, c->d_cur, 1, c->owner->fp ) != 1 )
197 |         return -1;
198 | 
199 |     c->d_cur = 0;
200 | 
201 |     return 0;
202 | }
203 | 
204 | static int mk_close_context( mk_context *c, unsigned *off )
205 | {
206 |     if( c->id )
207 |     {
208 |         CHECK( mk_write_id( c->parent, c->id ) );
209 |         CHECK( mk_write_size( c->parent, c->d_cur ) );
210 |     }
211 | 
212 |     if( c->parent && off )
213 |         *off += c->parent->d_cur;
214 | 
215 |     CHECK( mk_flush_context_data( c ) );
216 | 
217 |     if( c->next )
218 |         c->next->prev = c->prev;
219 |     *(c->prev) = c->next;
220 |     c->next = c->owner->freelist;
221 |     c->owner->freelist = c;
222 | 
223 |     return 0;
224 | }
225 | 
226 | static void mk_destroy_contexts( mk_writer *w )
227 | {
228 |     mk_context *next;
229 | 
230 |     mk_context * cur = 0;
231 |     for( cur = w->freelist; cur; cur = next )
232 |     {
233 |         next = cur->next;
234 |         free( cur->data );
235 |         free( cur );
236 |     }
237 | 
238 |     for( cur = w->actlist; cur; cur = next )
239 |     {
240 |         next = cur->next;
241 |         free( cur->data );
242 |         free( cur );
243 |     }
244 | 
245 |     w->freelist = w->actlist = w->root = NULL;
246 | }
247 | 
248 | static int mk_write_string( mk_context *c, unsigned id, const char *str )
249 | {
250 |     size_t len = strlen( str );
251 | 
252 |     CHECK( mk_write_id( c, id ) );
253 |     CHECK( mk_write_size( c, len ) );
254 |     CHECK( mk_append_context_data( c, str, len ) );
255 |     return 0;
256 | }
257 | 
258 | static int mk_write_bin( mk_context *c, unsigned id, const void *data, unsigned size )
259 | {
260 |     CHECK( mk_write_id( c, id ) );
261 |     CHECK( mk_write_size( c, size ) );
262 |     CHECK( mk_append_context_data( c, data, size ) ) ;
263 |     return 0;
264 | }
265 | 
266 | static int mk_write_uint( mk_context *c, unsigned id, int64_t ui )
267 | {
268 |     unsigned char c_ui[8] = { ui >> 56, ui >> 48, ui >> 40, ui >> 32, ui >> 24, ui >> 16, ui >> 8, ui };
269 |     unsigned i = 0;
270 | 
271 |     CHECK( mk_write_id( c, id ) );
272 |     while( i < 7 && !c_ui[i] )
273 |         ++i;
274 |     CHECK( mk_write_size( c, 8 - i ) );
275 |     CHECK( mk_append_context_data( c, c_ui+i, 8 - i ) );
276 |     return 0;
277 | }
278 | 
279 | static int mk_write_float_raw( mk_context *c, float f )
280 | {
281 |     union
282 |     {
283 |         float f;
284 |         unsigned u;
285 |     } u;
286 |     unsigned char c_f[4];
287 | 
288 |     u.f = f;
289 |     c_f[0] = u.u >> 24;
290 |     c_f[1] = u.u >> 16;
291 |     c_f[2] = u.u >> 8;
292 |     c_f[3] = u.u;
293 | 
294 |     return mk_append_context_data( c, c_f, 4 );
295 | }
296 | 
297 | static int mk_write_float( mk_context *c, unsigned id, float f )
298 | {
299 |     CHECK( mk_write_id( c, id ) );
300 |     CHECK( mk_write_size( c, 4 ) );
301 |     CHECK( mk_write_float_raw( c, f ) );
302 |     return 0;
303 | }
304 | 
305 | mk_writer *mk_create_writer( const char *filename )
306 | {
307 |     mk_writer *w = malloc( sizeof(*w) );
308 |     if( !w )
309 |         return NULL;
310 | 
311 |     memset( w, 0, sizeof(*w) );
312 | 
313 |     w->root = mk_create_context( w, NULL, 0 );
314 |     if( !w->root )
315 |     {
316 |         free( w );
317 |         return NULL;
318 |     }
319 | 
320 |     if( !strcmp( filename, "-" ) )
321 |         w->fp = stdout;
322 |     else
323 |         w->fp = fopen( filename, "wb" );
324 |     if( !w->fp )
325 |     {
326 |         mk_destroy_contexts( w );
327 |         free( w );
328 |         return NULL;
329 |     }
330 | 
331 |     w->timescale = 1000000;
332 | 
333 |     return w;
334 | }
335 | 
336 | int mk_write_header( mk_writer *w, const char *writing_app,
337 |                      const char *codec_id,
338 |                      const void *codec_private, unsigned codec_private_size,
339 |                      int64_t default_frame_duration,
340 |                      int64_t timescale,
341 |                      unsigned width, unsigned height,
342 |                      unsigned d_width, unsigned d_height, int display_size_units )
343 | {
344 |     mk_context  *c, *ti, *v;
345 | 
346 |     if( w->wrote_header )
347 |         return -1;
348 | 
349 |     w->timescale = timescale;
350 |     w->def_duration = default_frame_duration;
351 | 
352 |     if( !(c = mk_create_context( w, w->root, 0x1a45dfa3 )) ) // EBML
353 |         return -1;
354 |     CHECK( mk_write_uint( c, 0x4286, 1 ) ); // EBMLVersion
355 |     CHECK( mk_write_uint( c, 0x42f7, 1 ) ); // EBMLReadVersion
356 |     CHECK( mk_write_uint( c, 0x42f2, 4 ) ); // EBMLMaxIDLength
357 |     CHECK( mk_write_uint( c, 0x42f3, 8 ) ); // EBMLMaxSizeLength
358 |     CHECK( mk_write_string( c, 0x4282, "matroska") ); // DocType
359 |     CHECK( mk_write_uint( c, 0x4287, 2 ) ); // DocTypeVersion
360 |     CHECK( mk_write_uint( c, 0x4285, 2 ) ); // DocTypeReadversion
361 |     CHECK( mk_close_context( c, 0 ) );
362 | 
363 |     if( !(c = mk_create_context( w, w->root, 0x18538067 )) ) // Segment
364 |         return -1;
365 |     CHECK( mk_flush_context_id( c ) );
366 |     CHECK( mk_close_context( c, 0 ) );
367 | 
368 |     if( !(c = mk_create_context( w, w->root, 0x1549a966 )) ) // SegmentInfo
369 |         return -1;
370 |     CHECK( mk_write_string( c, 0x4d80, "Haali Matroska Writer b0" ) );
371 |     CHECK( mk_write_string( c, 0x5741, writing_app ) );
372 |     CHECK( mk_write_uint( c, 0x2ad7b1, w->timescale ) );
373 |     CHECK( mk_write_float( c, 0x4489, 0) );
374 |     w->duration_ptr = c->d_cur - 4;
375 |     CHECK( mk_close_context( c, &w->duration_ptr ) );
376 | 
377 |     if( !(c = mk_create_context( w, w->root, 0x1654ae6b )) ) // tracks
378 |         return -1;
379 |     if( !(ti = mk_create_context( w, c, 0xae )) ) // TrackEntry
380 |         return -1;
381 |     CHECK( mk_write_uint( ti, 0xd7, 1 ) ); // TrackNumber
382 |     CHECK( mk_write_uint( ti, 0x73c5, 1 ) ); // TrackUID
383 |     CHECK( mk_write_uint( ti, 0x83, 1 ) ); // TrackType
384 |     CHECK( mk_write_uint( ti, 0x9c, 0 ) ); // FlagLacing
385 |     CHECK( mk_write_string( ti, 0x86, codec_id ) ); // codec_id
386 |     if( codec_private_size )
387 |         CHECK( mk_write_bin( ti, 0x63a2, codec_private, codec_private_size ) ); // codec_private
388 |     if( default_frame_duration )
389 |         CHECK( mk_write_uint( ti, 0x23e383, default_frame_duration ) ); // DefaultDuration
390 | 
391 |     if( !(v = mk_create_context( w, ti, 0xe0 ) ) ) // Video
392 |         return -1;
393 |     CHECK( mk_write_uint( v, 0xb0, width ) );
394 |     CHECK( mk_write_uint( v, 0xba, height ) );
395 |     CHECK( mk_write_uint( v, 0x54b2, display_size_units ) );
396 |     CHECK( mk_write_uint( v, 0x54b0, d_width ) );
397 |     CHECK( mk_write_uint( v, 0x54ba, d_height ) );
398 |     CHECK( mk_close_context( v, 0 ) );
399 | 
400 |     CHECK( mk_close_context( ti, 0 ) );
401 | 
402 |     CHECK( mk_close_context( c, 0 ) );
403 | 
404 |     CHECK( mk_flush_context_data( w->root ) );
405 | 
406 |     w->wrote_header = 1;
407 | 
408 |     return 0;
409 | }
410 | 
411 | static int mk_close_cluster( mk_writer *w )
412 | {
413 |     if( w->cluster == NULL )
414 |         return 0;
415 |     CHECK( mk_close_context( w->cluster, 0 ) );
416 |     w->cluster = NULL;
417 |     CHECK( mk_flush_context_data( w->root ) );
418 |     return 0;
419 | }
420 | 
421 | static int mk_flush_frame( mk_writer *w )
422 | {
423 |     int64_t delta;
424 |     unsigned fsize;
425 |     unsigned char c_delta_flags[3];
426 | 
427 |     if( !w->in_frame )
428 |         return 0;
429 | 
430 |     delta = w->frame_tc/w->timescale - w->cluster_tc_scaled;
431 |     if( delta > 32767ll || delta < -32768ll )
432 |         CHECK( mk_close_cluster( w ) );
433 | 
434 |     if( !w->cluster )
435 |     {
436 |         w->cluster_tc_scaled = w->frame_tc / w->timescale;
437 |         w->cluster = mk_create_context( w, w->root, 0x1f43b675 ); // Cluster
438 |         if( !w->cluster )
439 |             return -1;
440 | 
441 |         CHECK( mk_write_uint( w->cluster, 0xe7, w->cluster_tc_scaled ) ); // Timecode
442 | 
443 |         delta = 0;
444 |     }
445 | 
446 |     fsize = w->frame ? w->frame->d_cur : 0;
447 | 
448 |     CHECK( mk_write_id( w->cluster, 0xa3 ) ); // SimpleBlock
449 |     CHECK( mk_write_size( w->cluster, fsize + 4 ) );
450 |     CHECK( mk_write_size( w->cluster, 1 ) ); // track number
451 | 
452 |     c_delta_flags[0] = delta >> 8;
453 |     c_delta_flags[1] = delta;
454 |     c_delta_flags[2] = (w->keyframe << 7) | w->skippable;
455 |     CHECK( mk_append_context_data( w->cluster, c_delta_flags, 3 ) );
456 |     if( w->frame )
457 |     {
458 |         CHECK( mk_append_context_data( w->cluster, w->frame->data, w->frame->d_cur ) );
459 |         w->frame->d_cur = 0;
460 |     }
461 | 
462 |     w->in_frame = 0;
463 | 
464 |     if( w->cluster->d_cur > CLSIZE )
465 |         CHECK( mk_close_cluster( w ) );
466 | 
467 |     return 0;
468 | }
469 | 
470 | int mk_start_frame( mk_writer *w )
471 | {
472 |     if( mk_flush_frame( w ) < 0 )
473 |         return -1;
474 | 
475 |     w->in_frame  = 1;
476 |     w->keyframe  = 0;
477 |     w->skippable = 0;
478 | 
479 |     return 0;
480 | }
481 | 
482 | int mk_set_frame_flags( mk_writer *w, int64_t timestamp, int keyframe, int skippable )
483 | {
484 |     if( !w->in_frame )
485 |         return -1;
486 | 
487 |     w->frame_tc  = timestamp;
488 |     w->keyframe  = keyframe  != 0;
489 |     w->skippable = skippable != 0;
490 | 
491 |     if( w->max_frame_tc < timestamp )
492 |         w->max_frame_tc = timestamp;
493 | 
494 |     return 0;
495 | }
496 | 
497 | int mk_add_frame_data( mk_writer *w, const void *data, unsigned size )
498 | {
499 |     if( !w->in_frame )
500 |         return -1;
501 | 
502 |     if( !w->frame )
503 |         if( !(w->frame = mk_create_context( w, NULL, 0 )) )
504 |         return -1;
505 | 
506 |     return mk_append_context_data( w->frame, data, size );
507 | }
508 | 
509 | int mk_close( mk_writer *w, int64_t last_delta )
510 | {
511 |     int ret = 0;
512 |     if( mk_flush_frame( w ) < 0 || mk_close_cluster( w ) < 0 )
513 |         ret = -1;
514 |     if( w->wrote_header && x264_is_regular_file( w->fp ) )
515 |     {
516 |         fseek( w->fp, w->duration_ptr, SEEK_SET );
517 |         int64_t last_frametime = w->def_duration ? w->def_duration : last_delta;
518 |         int64_t total_duration = w->max_frame_tc+last_frametime;
519 |         if( mk_write_float_raw( w->root, (float)((double)total_duration / w->timescale) ) < 0 ||
520 |             mk_flush_context_data( w->root ) < 0 )
521 |             ret = -1;
522 |     }
523 |     mk_destroy_contexts( w );
524 |     fclose( w->fp );
525 |     free( w );
526 |     return ret;
527 | }
528 | 


--------------------------------------------------------------------------------
/src/h264_receiver.cpp:
--------------------------------------------------------------------------------
  1 | #include <boost/asio.hpp>
  2 | #include <boost/thread.hpp>
  3 | 
  4 | #include <fstream>
  5 | 
  6 | #include <ros_h264_streamer/h264_receiver.h>
  7 | 
  8 | #include "private/net_buffer_size.h"
  9 | 
 10 | #include <ros_h264_streamer/h264_decoder.h>
 11 | 
 12 | #ifndef WIN32
 13 | #include <image_transport/image_transport.h>
 14 | #else
 15 | void sleep(unsigned int sec)
 16 | {
 17 |   Sleep(1000*sec);
 18 | }
 19 | #endif
 20 | 
 21 | #include "private/net_buffer_size.h"
 22 | 
 23 | using boost::asio::ip::udp;
 24 | using boost::asio::ip::tcp;
 25 | 
 26 | namespace ros_h264_streamer
 27 | {
 28 | 
 29 | enum H264ReceiverProtocol
 30 | {
 31 |   ChunkIDPlusData = 1,
 32 |   FrameSizePlusData
 33 | };
 34 | 
 35 | struct H264ReceiverNetImpl
 36 | {
 37 |   #ifndef WIN32
 38 |   H264ReceiverNetImpl(H264Receiver::Config & conf, ros::NodeHandle & nh)
 39 |   #else
 40 |   H264ReceiverNetImpl(H264Receiver::Config & conf)
 41 |   #endif
 42 |   : io_service(), io_service_th(0), stop_io_service(false), request_data(0), protocol(ChunkIDPlusData), next_chunkID(0), video_chunk_size(0), chunk_data(0),
 43 |     frame_data_size(0), full_frame_data_size(0), frame_data(0),
 44 |     has_new_data(false), img(new sensor_msgs::Image),
 45 |     decoder(conf.width, conf.height)
 46 |     #ifndef WIN32
 47 |     , publish(conf.publish), it(nh), pub()
 48 |     #endif
 49 |     , debug_log()
 50 |   {
 51 |       debug_log.open("h264_receiver.log", std::ios::out);
 52 |       debug_log << "Start H264Receiver with config: " << std::endl
 53 |                 << "UDP: " << conf.udp << std::endl
 54 |                 << "Server: " << conf.server << std::endl
 55 |                 << "Host: " << conf.host << " Port: " << conf.port << std::endl
 56 |                 << "WidthxHeight: " << conf.width << "x" << conf.height << std::endl << std::endl;
 57 |   }
 58 | 
 59 |   void InitBuffers(H264Receiver::Config & conf)
 60 |   {
 61 |     SetVideoChunkSize();
 62 |     request_data = new char[ros_h264_streamer_private::_request_size];
 63 |     chunk_data = new unsigned char[video_chunk_size];
 64 |     frame_data_size = 0;
 65 |     frame_data = new uint8_t[full_frame_data_size];
 66 |     CleanRequestData();
 67 |     CleanChunkData();
 68 |     CleanFrameData();
 69 | 
 70 |     #ifndef WIN32
 71 |     if(publish)
 72 |     {
 73 |       img->header.seq = 0;
 74 |       img->header.frame_id = conf.frame_id;
 75 |       pub = it.advertise(conf.publish_topic, 1);
 76 |     }
 77 |     #endif
 78 |   }
 79 | 
 80 |   ~H264ReceiverNetImpl()
 81 |   {
 82 |     io_service.stop();
 83 |     if(io_service_th)
 84 |     {
 85 |       io_service_th->join();
 86 |       delete io_service_th;
 87 |     }
 88 |     delete[] request_data;
 89 |     delete[] chunk_data;
 90 |     delete[] frame_data;
 91 |   }
 92 | 
 93 |   virtual void SetVideoChunkSize() = 0;
 94 | 
 95 |   virtual void ReceiveMissingData(size_t bytes_recvd, size_t missing_data_size) {}
 96 | 
 97 |   void IOServiceThread()
 98 |   {
 99 |       while(!stop_io_service)
100 |       {
101 |           io_service.run();
102 |           io_service.reset();
103 |       }
104 |   }
105 | 
106 |   void StartIOService()
107 |   {
108 |       io_service_th = new boost::thread(boost::bind(&H264ReceiverNetImpl::IOServiceThread, this));
109 |   }
110 | 
111 |   void ResizeFrameData()
112 |   {
113 |     std::cerr << "[ros_h264_streamer] H264Receiver needs to re-allocate frame data" << std::endl;
114 |     uint8_t * old_frame_data = frame_data;
115 |     uint8_t * new_frame_data = new uint8_t[2*frame_data_size];
116 |     memcpy(new_frame_data, frame_data, full_frame_data_size);
117 |     full_frame_data_size = 2*frame_data_size;
118 |     frame_data = new_frame_data;
119 |     delete[] old_frame_data;
120 |   }
121 | 
122 |   bool HandleVideoChunk(size_t bytes_recvd)
123 |   {
124 |     bool decode_frame_data = false;
125 |     if(protocol == ChunkIDPlusData)
126 |     {
127 |       debug_log << "Handling chunk of size " << bytes_recvd << std::endl;
128 |       uint8_t chunkID = chunk_data[0];
129 |       debug_log << "ChunkID is " << (int)chunkID << std::endl;
130 |       frame_data_size += bytes_recvd - 1;
131 |       if(frame_data_size > full_frame_data_size)
132 |       {
133 |         ResizeFrameData();
134 |       }
135 |       if(chunkID*(video_chunk_size-1) + bytes_recvd - 1 < full_frame_data_size && chunkID == next_chunkID)
136 |       {
137 |         memcpy(&frame_data[chunkID*(video_chunk_size-1)], &chunk_data[1], bytes_recvd - 1);
138 |         next_chunkID++;
139 |         if(bytes_recvd < video_chunk_size)
140 |         {
141 |           decode_frame_data = true;
142 |           next_chunkID = 0;
143 |         }
144 |       }
145 |       else
146 |       {
147 |           debug_log << "Ignoring data with ChunkID " << (int)chunkID << std::endl;
148 |           frame_data_size = 0;
149 |           CleanFrameData();
150 |           next_chunkID = 0;
151 |       }
152 |       CleanChunkData();
153 |     }
154 |     else
155 |     {
156 |       memcpy(&frame_data_size, chunk_data, sizeof(int));
157 |       memcpy(frame_data, &chunk_data[sizeof(int)], bytes_recvd - sizeof(int));
158 |       CleanChunkData();
159 |       if(frame_data_size < 0 || bytes_recvd > frame_data_size + sizeof(int) || frame_data_size > full_frame_data_size)
160 |       {
161 |         debug_log << "[debug] Got fishy data, ignoring this package (bytes recvd: " << bytes_recvd << ", frame_data_size: " << frame_data_size << ", full_frame_data_size: " << full_frame_data_size << ")" << std::endl;
162 |         CleanFrameData();
163 |         return false;
164 |       }
165 |       if(bytes_recvd != frame_data_size + sizeof(int))
166 |       {
167 |         debug_log << "[debug] Received " << bytes_recvd << ", frame size is " << frame_data_size << ", will receive more data now" << std::endl;
168 |         ReceiveMissingData(bytes_recvd, frame_data_size + sizeof(int) - bytes_recvd);
169 |       }
170 |       debug_log << "[debug] Received enough data to decode frame" << std::endl;
171 |       decode_frame_data = true;
172 |     }
173 |     if(decode_frame_data)
174 |     {
175 |       boost::mutex::scoped_lock lock(img_mutex);
176 |       int data_decoded = decoder.decode(frame_data_size, frame_data, img);
177 |       debug_log << "[debug] Picture decoded, picture size: " << data_decoded << std::endl;
178 |       frame_data_size = 0;
179 |       if(data_decoded > 0)
180 |       {
181 |         has_new_data = true;
182 |         #ifndef WIN32
183 |         if(publish)
184 |         {
185 |           img->header.seq++;
186 |           img->header.stamp = ros::Time::now();
187 |           pub.publish(*img);
188 |         }
189 |         #endif
190 |       }
191 |       CleanFrameData();
192 |     }
193 |     return true;
194 |   }
195 | 
196 |   bool getLatestImage(sensor_msgs::ImagePtr & img_in)
197 |   {
198 |     if(has_new_data)
199 |     {
200 |       boost::mutex::scoped_lock lock(img_mutex);
201 |       debug_log << "[debug] Copying received image into display image" << std::endl;
202 |       *img_in = *img;
203 |       debug_log << "[debug] Done copying" << std::endl;
204 |       has_new_data = false;
205 |       return true;
206 |     }
207 |     return false;
208 |   }
209 | 
210 |   boost::asio::io_service io_service;
211 |   boost::thread * io_service_th;
212 |   bool stop_io_service;
213 | 
214 |   char * request_data;
215 |   void CleanRequestData() { memset(request_data, 0, ros_h264_streamer_private::_request_size); }
216 |   unsigned char * chunk_data;
217 |   H264ReceiverProtocol protocol;
218 |   uint8_t next_chunkID;
219 |   int video_chunk_size;
220 |   void CleanChunkData() { memset(chunk_data, 0, video_chunk_size); }
221 | 
222 |   int frame_data_size;
223 |   int full_frame_data_size;
224 |   uint8_t * frame_data;
225 |   void CleanFrameData() { memset(frame_data, 0, full_frame_data_size); }
226 |   bool has_new_data;
227 |   sensor_msgs::ImagePtr img;
228 |   boost::mutex img_mutex;
229 |   H264Decoder decoder;
230 | 
231 |   #ifndef WIN32
232 |   bool publish;
233 |   image_transport::ImageTransport it;
234 |   image_transport::Publisher pub;
235 |   #endif
236 | 
237 |   std::ofstream debug_log;
238 | };
239 | 
240 | struct H264ReceiverUDPServer : public H264ReceiverNetImpl
241 | {
242 |   #ifndef WIN32
243 |   H264ReceiverUDPServer(H264Receiver::Config & conf, ros::NodeHandle & nh)
244 |   : H264ReceiverNetImpl(conf, nh),
245 |   #else
246 |   H264ReceiverUDPServer(H264Receiver::Config & conf)
247 |   : H264ReceiverNetImpl(conf),
248 |   #endif
249 |     socket(0), client_endpoint()
250 |   {
251 |     socket = new udp::socket(io_service);
252 |     socket->open(udp::v4());
253 |     socket->bind(udp::endpoint(udp::v4(), conf.port));
254 |     socket->async_receive_from(
255 |       boost::asio::buffer(chunk_data, video_chunk_size), client_endpoint,
256 |       boost::bind(&H264ReceiverUDPServer::handle_receive_from, this,
257 |         boost::asio::placeholders::error,
258 |         boost::asio::placeholders::bytes_transferred));
259 |   }
260 | 
261 |   ~H264ReceiverUDPServer()
262 |   {
263 |     socket->close();
264 |     delete socket;
265 |   }
266 | 
267 |   void SetVideoChunkSize()
268 |   {
269 |     video_chunk_size = ros_h264_streamer_private::_udp_video_chunk_size;
270 |     full_frame_data_size = 3*video_chunk_size;
271 |   }
272 | 
273 |   void handle_receive_from(const boost::system::error_code & error, size_t bytes_recvd)
274 |   {
275 |     if(!error && bytes_recvd > 0)
276 |     {
277 |       HandleVideoChunk(bytes_recvd);
278 |     }
279 |     else if(error)
280 |     {
281 |       std::cerr << "[ros_h264_streamer] H264Receiver UDP server got the error while receiving data: " << std::endl << error.message() << std::endl;
282 |     }
283 |     socket->async_receive_from(
284 |       boost::asio::buffer(chunk_data, video_chunk_size), client_endpoint,
285 |       boost::bind(&H264ReceiverUDPServer::handle_receive_from, this,
286 |         boost::asio::placeholders::error,
287 |         boost::asio::placeholders::bytes_transferred));
288 |   }
289 | 
290 | private:
291 |   udp::socket * socket;
292 |   udp::endpoint client_endpoint;
293 | };
294 | 
295 | struct H264ReceiverUDPClient : public H264ReceiverNetImpl
296 | {
297 |   #ifndef WIN32
298 |   H264ReceiverUDPClient(H264Receiver::Config & conf, ros::NodeHandle & nh)
299 |   : H264ReceiverNetImpl(conf, nh),
300 |   #else
301 |   H264ReceiverUDPClient(H264Receiver::Config & conf)
302 |   : H264ReceiverNetImpl(conf),
303 |   #endif
304 |     socket(0), server_endpoint(), client_endpoint(),
305 |     timeout_timer(io_service, boost::posix_time::seconds(1))
306 |   {
307 |     udp::resolver resolver(io_service);
308 |     std::stringstream ss;
309 |     ss << conf.port;
310 |     udp::resolver::query query(udp::v4(), conf.host, ss.str());
311 |     server_endpoint = *resolver.resolve(query);
312 | 
313 |     socket = new udp::socket(io_service);
314 |     socket->open(udp::v4());
315 |     SendVideoRequest();
316 |   }
317 | 
318 |   void SetVideoChunkSize()
319 |   {
320 |     video_chunk_size = ros_h264_streamer_private::_udp_video_chunk_size;
321 |     full_frame_data_size = 3*video_chunk_size;
322 |   }
323 | 
324 |   void SendVideoRequest()
325 |   {
326 |     std::string request_ = "get";
327 |     socket->async_send_to(
328 |       boost::asio::buffer(request_.c_str(), request_.size() + 1),
329 |       server_endpoint,
330 |       boost::bind(&H264ReceiverUDPClient::handle_send_to, this,
331 |         boost::asio::placeholders::error,
332 |         boost::asio::placeholders::bytes_transferred));
333 |   }
334 | 
335 |   void ReceiveData()
336 |   {
337 |     timeout_timer.cancel();
338 |     socket->async_receive_from(
339 |       boost::asio::buffer(chunk_data, video_chunk_size), client_endpoint,
340 |       boost::bind(&H264ReceiverUDPClient::handle_receive_from, this,
341 |         boost::asio::placeholders::error,
342 |         boost::asio::placeholders::bytes_transferred));
343 |     timeout_timer.expires_from_now(boost::posix_time::seconds(1));
344 |     timeout_timer.async_wait(boost::bind(&H264ReceiverUDPClient::handle_timeout, this, boost::asio::placeholders::error));
345 |   }
346 | 
347 |   void handle_send_to(const boost::system::error_code & error, size_t bytes_send)
348 |   {
349 |     if(!error)
350 |     {
351 |       ReceiveData();
352 |     }
353 |     else
354 |     {
355 |       std::cerr << "[ros_h264_streamer] H264Receiver UDP client error when trying to send to server: " << std::endl << error.message() << std::endl;
356 |       std::cerr << "[ros_h264_streamer] Retrying in one second" << std::endl;
357 |       sleep(1);
358 |       SendVideoRequest();
359 |     }
360 |   }
361 | 
362 |   void handle_receive_from(const boost::system::error_code & error, size_t bytes_recvd)
363 |   {
364 |     if(!error && bytes_recvd > 0)
365 |     {
366 |       HandleVideoChunk(bytes_recvd);
367 |       ReceiveData();
368 |     }
369 |     else if(error)
370 |     {
371 |       std::cerr << "[ros_h264_streamer] H264Receiver UDP server got the error while receiving data: " << std::endl << error.message() << std::endl;
372 |       SendVideoRequest();
373 |     }
374 |   }
375 | 
376 |   void handle_timeout(const boost::system::error_code & error)
377 |   {
378 |     if(error != boost::asio::error::operation_aborted)
379 |     {
380 |       std::cerr << "[ros_h264_streamer] Video reception timeout, sending another request" << std::endl;
381 |       SendVideoRequest();
382 |     }
383 |   }
384 | 
385 | private:
386 |   udp::socket * socket;
387 |   udp::endpoint server_endpoint;
388 |   udp::endpoint client_endpoint;
389 |   boost::asio::deadline_timer timeout_timer;
390 | };
391 | 
392 | struct H264ReceiverTCPServer : public H264ReceiverNetImpl
393 | {
394 |   #ifndef WIN32
395 |   H264ReceiverTCPServer(H264Receiver::Config & conf, ros::NodeHandle & nh)
396 |   : H264ReceiverNetImpl(conf, nh),
397 |   #else
398 |   H264ReceiverTCPServer(H264Receiver::Config & conf)
399 |   : H264ReceiverNetImpl(conf),
400 |   #endif
401 |     socket(0), acceptor(io_service, tcp::endpoint(tcp::v4(), conf.port))
402 |   {
403 |     AcceptConnection();
404 |   }
405 | 
406 |   void SetVideoChunkSize()
407 |   {
408 |     video_chunk_size = ros_h264_streamer_private::_tcp_video_chunk_size;
409 |     full_frame_data_size = 90*video_chunk_size;
410 |   }
411 | 
412 |   ~H264ReceiverTCPServer()
413 |   {
414 |     acceptor.close();
415 |     if(socket)
416 |     {
417 |       socket->close();
418 |     }
419 |     delete socket;
420 |   }
421 | 
422 |   void ReceiveData()
423 |   {
424 |     if(socket)
425 |     {
426 |       socket->async_receive(
427 |         boost::asio::buffer(chunk_data, video_chunk_size),
428 |         boost::bind(&H264ReceiverTCPServer::handle_receive, this,
429 |           boost::asio::placeholders::error,
430 |           boost::asio::placeholders::bytes_transferred));
431 |     }
432 |   }
433 | 
434 |   void AcceptConnection()
435 |   {
436 |     tcp::socket * nsocket = new tcp::socket(io_service);
437 |     acceptor.async_accept(*nsocket,
438 |       boost::bind(&H264ReceiverTCPServer::handle_accept, this, nsocket, boost::asio::placeholders::error));
439 |   }
440 | 
441 |   void handle_accept(tcp::socket * socket_in, const boost::system::error_code& error)
442 |   {
443 |     delete socket;
444 |     if(!error)
445 |     {
446 |       socket = socket_in;
447 |       ReceiveData();
448 |     }
449 |     AcceptConnection();
450 |   }
451 | 
452 |   void ReceiveMissingData(size_t bytes_recvd, size_t missing_data_size)
453 |   {
454 |     if(socket)
455 |     {
456 |       size_t data_left = missing_data_size;
457 |       while(data_left > 0)
458 |       {
459 |         int nbytes_recvd = socket->read_some(boost::asio::buffer(chunk_data, std::min((size_t)video_chunk_size, data_left)));
460 |         memcpy(&frame_data[bytes_recvd-sizeof(int)], chunk_data, nbytes_recvd);
461 |         CleanChunkData();
462 |         data_left -= nbytes_recvd;
463 |         bytes_recvd += nbytes_recvd;
464 |       }
465 |     }
466 |   }
467 | 
468 |   void handle_receive(const boost::system::error_code & error, size_t bytes_recvd)
469 |   {
470 |     if(!error && bytes_recvd > 0)
471 |     {
472 |       HandleVideoChunk(bytes_recvd);
473 |       ReceiveData();
474 |     }
475 |     else if(error)
476 |     {
477 |       std::cerr << "[ros_h264_streamer] H264Receiver TCP server got the error while receiving data: " << std::endl << error.message() << std::endl;
478 |     }
479 |   }
480 | 
481 | private:
482 |   tcp::socket * socket;
483 |   tcp::acceptor acceptor;
484 | };
485 | 
486 | struct H264ReceiverTCPClient : public H264ReceiverNetImpl
487 | {
488 |   #ifndef WIN32
489 |   H264ReceiverTCPClient(H264Receiver::Config & conf, ros::NodeHandle & nh)
490 |   : H264ReceiverNetImpl(conf, nh), socket(0),
491 |   #else
492 |   H264ReceiverTCPClient(H264Receiver::Config & conf)
493 |   : H264ReceiverNetImpl(conf), socket(0),
494 |   #endif
495 |     timeout_timer(io_service, boost::posix_time::seconds(1))
496 |   {
497 |     tcp::resolver resolver(io_service);
498 |     std::stringstream ss;
499 |     ss << conf.port;
500 |     tcp::resolver::query query(tcp::v4(), conf.host, ss.str());
501 |     server_endpoint = *resolver.resolve(query);
502 | 
503 |     ConnectToServer();
504 |   }
505 | 
506 |   void SetVideoChunkSize()
507 |   {
508 |     video_chunk_size = ros_h264_streamer_private::_tcp_video_chunk_size;
509 |     full_frame_data_size = 90*video_chunk_size;
510 |   }
511 | 
512 |   void ConnectToServer()
513 |   {
514 |     delete socket;
515 |     socket = new tcp::socket(io_service);
516 |     debug_log << "[debug] Connecting to " << server_endpoint << std::endl;
517 |     socket->async_connect(server_endpoint,
518 |       boost::bind(&H264ReceiverTCPClient::handle_connect, this,
519 |         boost::asio::placeholders::error));
520 |     timeout_timer.expires_from_now(boost::posix_time::seconds(5));
521 |     timeout_timer.async_wait(boost::bind(&H264ReceiverTCPClient::handle_timeout, this, boost::asio::placeholders::error));
522 |   }
523 | 
524 |   void ReceiveData()
525 |   {
526 |     socket->async_receive(
527 |       boost::asio::buffer(chunk_data, video_chunk_size), 0,
528 |       boost::bind(&H264ReceiverTCPClient::handle_receive, this,
529 |         boost::asio::placeholders::error,
530 |         boost::asio::placeholders::bytes_transferred));
531 |   }
532 | 
533 |   void ReceiveMissingData(size_t bytes_recvd, size_t missing_data_size)
534 |   {
535 |     if(socket)
536 |     {
537 |       size_t data_left = missing_data_size;
538 |       while(data_left > 0)
539 |       {
540 |         int nbytes_recvd = socket->read_some(boost::asio::buffer(chunk_data, std::min((size_t)video_chunk_size, data_left)));
541 |         memcpy(&frame_data[bytes_recvd-sizeof(int)], chunk_data, nbytes_recvd);
542 |         CleanChunkData();
543 |         data_left -= nbytes_recvd;
544 |         bytes_recvd += nbytes_recvd;
545 |       }
546 |     }
547 |   }
548 | 
549 |   void handle_connect(const boost::system::error_code & error)
550 |   {
551 |     timeout_timer.cancel();
552 |     if(!error)
553 |     {
554 |       debug_log << "[debug] Connection succeed" << std::endl;
555 |       ReceiveData();
556 |     }
557 |     else
558 |     {
559 |       debug_log << "[debug] Connection failed with error: " << error.message() << std::endl;
560 |       sleep(1);
561 |       ConnectToServer();
562 |     }
563 |   }
564 | 
565 |   void handle_timeout(const boost::system::error_code & error)
566 |   {
567 |     if(error != boost::asio::error::operation_aborted)
568 |     {
569 |       debug_log << "[debug] Timeout during attempted connection, trying again" << std::endl;
570 |       ConnectToServer();
571 |     }
572 |   }
573 | 
574 |   void handle_receive(const boost::system::error_code & error, size_t bytes_recvd)
575 |   {
576 |     if(!error && bytes_recvd > 0)
577 |     {
578 |       if(HandleVideoChunk(bytes_recvd))
579 |       {
580 |         ReceiveData();
581 |       }
582 |       else
583 |       {
584 |         socket->close();
585 |         ConnectToServer();
586 |       }
587 |     }
588 |     else if(error)
589 |     {
590 |       std::cerr << "[ros_h264_streamer] H264Receiver TCP client got the error while receiving data: " << std::endl << error.message() << std::endl;
591 |       ConnectToServer();
592 |     }
593 |   }
594 | 
595 | private:
596 |   tcp::socket * socket;
597 |   tcp::endpoint server_endpoint;
598 |   boost::asio::deadline_timer timeout_timer;
599 | };
600 | 
601 | struct H264ReceiverImpl
602 | {
603 | public:
604 |   #ifndef WIN32
605 |   H264ReceiverImpl(H264Receiver::Config & conf, ros::NodeHandle & nh)
606 |   #else
607 |   H264ReceiverImpl(H264Receiver::Config & conf)
608 |   #endif
609 |   : net_impl(0)
610 |   {
611 |     if(conf.udp)
612 |     {
613 |       if(conf.server)
614 |       {
615 |         #ifndef WIN32
616 |         net_impl = new H264ReceiverUDPServer(conf, nh);
617 |         #else
618 |         net_impl = new H264ReceiverUDPServer(conf);
619 |         #endif
620 |       }
621 |       else
622 |       {
623 |         #ifndef WIN32
624 |         net_impl = new H264ReceiverUDPClient(conf, nh);
625 |         #else
626 |         net_impl = new H264ReceiverUDPClient(conf);
627 |         #endif
628 |       }
629 |       net_impl->protocol = ChunkIDPlusData;
630 |     }
631 |     else
632 |     {
633 |       if(conf.server)
634 |       {
635 |         #ifndef WIN32
636 |         net_impl = new H264ReceiverTCPServer(conf, nh);
637 |         #else
638 |         net_impl = new H264ReceiverTCPServer(conf);
639 |         #endif
640 |       }
641 |       else
642 |       {
643 |         #ifndef WIN32
644 |         net_impl = new H264ReceiverTCPClient(conf, nh);
645 |         #else
646 |         net_impl = new H264ReceiverTCPClient(conf);
647 |         #endif
648 |       }
649 |       net_impl->protocol = ChunkIDPlusData;
650 |     }
651 |   }
652 | 
653 |   ~H264ReceiverImpl()
654 |   {
655 |     delete net_impl;
656 |   }
657 | 
658 |   void Init(H264Receiver::Config & conf)
659 |   {
660 |     net_impl->InitBuffers(conf);
661 |     net_impl->StartIOService();
662 |   }
663 | 
664 |   bool getLatestImage(sensor_msgs::ImagePtr & img)
665 |   {
666 |     return net_impl->getLatestImage(img);
667 |   }
668 | private:
669 |   H264ReceiverNetImpl * net_impl;
670 | };
671 | 
672 | #ifndef WIN32
673 | H264Receiver::H264Receiver(H264Receiver::Config & conf, ros::NodeHandle & nh)
674 | : impl(new H264ReceiverImpl(conf, nh))
675 | #else
676 | H264Receiver::H264Receiver(H264Receiver::Config & conf)
677 | : impl(new H264ReceiverImpl(conf))
678 | #endif
679 | {
680 |   impl->Init(conf);
681 | }
682 | 
683 | bool H264Receiver::getLatestImage(sensor_msgs::ImagePtr & img)
684 | {
685 |   return impl->getLatestImage(img);
686 | }
687 | 
688 | } // namespace ros_h264_Receiver
689 | 


--------------------------------------------------------------------------------