diff --git a/CMakeLists.txt b/CMakeLists.txt index 142baac..3314868 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 2.8.3) project(video_stream_opencv) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -O0 -std=c++11") + find_package(catkin REQUIRED COMPONENTS roscpp rospy @@ -10,8 +12,8 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs ) - -find_package(OpenCV) +find_package(OpenCV REQUIRED) +find_package(Curses REQUIRED) catkin_package() @@ -20,8 +22,10 @@ include_directories( ${OpenCV_INCLUDE_DIRS} ) +message("CURSES_LIBRARIES: ${CURSES_LIBRARIES}") + add_executable(video_stream src/video_stream.cpp) -target_link_libraries(video_stream ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) +target_link_libraries(video_stream ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${CURSES_LIBRARIES}) install(TARGETS video_stream RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/launch/camera.launch b/launch/camera.launch index 2e28307..b265de7 100644 --- a/launch/camera.launch +++ b/launch/camera.launch @@ -28,7 +28,8 @@ - + + diff --git a/launch/mjpg_stream_python.launch b/launch/mjpg_stream_python.launch new file mode 100644 index 0000000..ddf3799 --- /dev/null +++ b/launch/mjpg_stream_python.launch @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/video_file_jpg.launch b/launch/video_file_jpg.launch new file mode 100644 index 0000000..e15d293 --- /dev/null +++ b/launch/video_file_jpg.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/mjpeg-stream-to-ros-topic.py b/src/mjpeg-stream-to-ros-topic.py new file mode 100755 index 0000000..62cd68a --- /dev/null +++ b/src/mjpeg-stream-to-ros-topic.py @@ -0,0 +1,138 @@ +#!/usr/bin/env python +# coding=UTF-8 + +import cv2 +import urllib +import numpy as np + +import roslib +roslib.load_manifest('video_stream_opencv') +import signal +import sys +import time +import rospy +import yaml +import argparse + +from cv_bridge import CvBridge, CvBridgeError +#from sensor_msgs.msg import Image +from sensor_msgs.msg import CompressedImage +from sensor_msgs.msg import CameraInfo + +print(sys.version) +print(sys.argv) + +#args.topic_name = 'mjpeg_publisher' +#args.stream_url = 'http://iris.not.iac.es/axis-cgi/mjpg/video.cgi?resolution=320x240' +#args.jpeg_quality = 40 +#args.est_image_size = 409600 +#args.show_gui = False +#args.verbose = True +#args.caminfo_file = '/home/biagio/.ros/camera_info/vivotek_IB8168_C.yaml' + +parser = argparse.ArgumentParser(description='Given a MJPEG HTTP stream, this node publishes a CompressedImage (and its CameraInfo) topics.') +parser.add_argument('stream_url', help='The MJPEG URL', metavar='http://vivotek-0:8080/video3.mjpg', + default='http://vivotek-0:8080/video.mjpg') +parser.add_argument('topic_name', help='The output topic', metavar='image', + default='vivotek_0') +parser.add_argument('-q', help='The jpeg quality for an optional republish', dest='jpeg_quality', type=int, + metavar='40') +parser.add_argument('est_image_size', help='The estimated image size, to optimize recv speed', type=int, + metavar='409600') +parser.add_argument('--show_gui', help='Show the images that are being received', dest='show_gui', type=bool, + metavar=False, default=False) +parser.add_argument('-v', help='Be more verbose', dest='verbose', type=bool, + metavar=False, default=False) +parser.add_argument('-c', help='Camera info file', metavar='~/.ros/camera_info/camera.yaml', dest='caminfo_file', default='') + +args, unknown = parser.parse_known_args() + +print("URL: ", args.stream_url, "Output topic: ", args.topic_name, "Est. image size: ", args.est_image_size, "Camera info file: ", args.caminfo_file, "Jpeg quality: ", args.jpeg_quality, "Show GUI: ", args.show_gui, "Verbose: ", args.verbose) + + +def parse_calibration_yaml(calib_file): + with file(calib_file, 'r') as f: + params = yaml.load(f) + + cam_info = CameraInfo() + cam_info.header.frame_id = params['camera_name'] + cam_info.height = params['image_height'] + cam_info.width = params['image_width'] + cam_info.distortion_model = params['distortion_model'] + cam_info.K = params['camera_matrix']['data'] + cam_info.D = params['distortion_coefficients']['data'] + cam_info.R = params['rectification_matrix']['data'] + cam_info.P = params['projection_matrix']['data'] + + return cam_info + + +stream = urllib.urlopen(args.stream_url) + +rospy.init_node('mjpeg_stream_to_ros_topic', anonymous=True) +mjpeg_publisher = rospy.Publisher (args.topic_name + '/compressed' , CompressedImage, queue_size = 1) +if args.jpeg_quality > 0: + low_qual_republisher = rospy.Publisher (args.topic_name + '_low_qual' + '/compressed', CompressedImage, queue_size = 1) + +if args.caminfo_file != '': + cam_pub = rospy.Publisher("camera_info", CameraInfo, queue_size = 1) + cam_info = parse_calibration_yaml(args.caminfo_file) +else: + cam_pub = None + +def signal_handler(sig, frame): + print('Ctrl+C pressed, exiting...') + sys.exit(0) + +def jpeg_publisher(data, publisher, cam_pub = None): + stamp = rospy.Time.now() + if cam_pub is not None: + cam_info.header.stamp = stamp + # publish the camera info messages first + cam_pub.publish(cam_info) + #### Create CompressedIamge #### + msg = CompressedImage() + msg.header.stamp = stamp + msg.format = "jpeg" + #msg.format = "rgb8; jpeg compressed bgr8" + #msg.data = np.array(cv2.imencode('.jpg', self.last_img)[1]).tostring() + msg.data = data.tostring() + # Publish new image + publisher.publish(msg) + + + +signal.signal(signal.SIGINT, signal_handler) +print('Press Ctrl+C to quit') + +bytes = '' +a = b = -1 +while True: + bytes += stream.read(args.est_image_size) + if a == -1: + a = bytes.find('\xff\xd8') + if b == -1: + b = bytes.find('\xff\xd9') + if a != -1 and b != -1: + jpg = bytes[a:b+2] + bytes = bytes[b+2:] + #print("----------------------------------------------------------", a, b, len(bytes)) + + a = b = -1 + + numpy_data = np.fromstring(jpg, dtype=np.uint8) + if args.show_gui: + i = cv2.imdecode(numpy_data, cv2.IMREAD_COLOR) + cv2.imshow('img', i) + if cv2.waitKey(1) == 27: + exit(0) + + if args.jpeg_quality > 0: + i = cv2.imdecode(numpy_data, cv2.IMREAD_COLOR) + retval, jpeg_data = cv2.imencode('.jpg', i, [cv2.IMWRITE_JPEG_QUALITY, args.jpeg_quality]) + if args.verbose: + print("Successful recompression: {} - orig jpeg: {} - recompressed: {}".format(retval, numpy_data.size, jpeg_data.size)) + if retval: + jpeg_publisher(jpeg_data, low_qual_republisher) + + jpeg_publisher(numpy_data, mjpeg_publisher, cam_pub) diff --git a/src/video_stream.cpp b/src/video_stream.cpp index 999a540..861a5d8 100644 --- a/src/video_stream.cpp +++ b/src/video_stream.cpp @@ -41,16 +41,25 @@ #include #include #include +#include #include #include #include +#define HAVE_NCURSES + +#ifdef HAVE_NCURSES +#include +#endif + boost::sync_queue framesQueue; cv::VideoCapture cap; -std::string camera_name; +std::string video_stream_provider_type; double set_camera_fps; int max_queue_size; +std::string ncurses_cr("\r"); + // Based on the ros tutorial on transforming opencv images to Image messages sensor_msgs::CameraInfo get_default_camera_info_from_image(sensor_msgs::ImagePtr img){ @@ -59,8 +68,8 @@ sensor_msgs::CameraInfo get_default_camera_info_from_image(sensor_msgs::ImagePtr // Fill image size cam_info_msg.height = img->height; cam_info_msg.width = img->width; - ROS_INFO_STREAM("The image width is: " << img->width); - ROS_INFO_STREAM("The image height is: " << img->height); + ROS_INFO_STREAM("The image width is: " << img->width << ncurses_cr); + ROS_INFO_STREAM("The image height is: " << img->height << ncurses_cr); // Add the most common distortion model as sensor_msgs/CameraInfo says cam_info_msg.distortion_model = "plumb_bob"; // Don't let distorsion matrix be empty @@ -90,7 +99,7 @@ void do_capture(ros::NodeHandle &nh) { // Read frames as fast as possible while (nh.ok()) { cap >> frame; - if (camera_name == "videofile") + if (video_stream_provider_type == "videofile") { camera_fps_rate.sleep(); } @@ -111,6 +120,13 @@ void do_capture(ros::NodeHandle &nh) { int main(int argc, char** argv) { +#ifdef HAVE_NCURSES + initscr(); + noecho(); + timeout(1); // Timeout for getch() + boost::this_thread::sleep_for(boost::chrono::duration(100)); +#endif + ros::init(argc, argv, "image_publisher"); ros::NodeHandle nh; ros::NodeHandle _nh("~"); // to get the private params @@ -120,15 +136,33 @@ int main(int argc, char** argv) // provider can be an url (e.g.: rtsp://10.0.0.1:554) or a number of device, (e.g.: 0 would be /dev/video0) std::string video_stream_provider; if (_nh.getParam("video_stream_provider", video_stream_provider)){ - ROS_INFO_STREAM("Resource video_stream_provider: " << video_stream_provider); + ROS_INFO_STREAM("Resource video_stream_provider: " << video_stream_provider << ncurses_cr); // If we are given a string of 4 chars or less (I don't think we'll have more than 100 video devices connected) // treat is as a number and act accordingly so we open up the videoNUMBER device if (video_stream_provider.size() < 4){ - ROS_INFO_STREAM("Getting video from provider: /dev/video" << video_stream_provider); + ROS_INFO_STREAM("Getting video from provider: /dev/video" << video_stream_provider << ncurses_cr); + video_stream_provider_type = "videodevice"; cap.open(atoi(video_stream_provider.c_str())); } else{ - ROS_INFO_STREAM("Getting video from provider: " << video_stream_provider); + ROS_INFO_STREAM("Getting video from provider: " << video_stream_provider << ncurses_cr); + if (video_stream_provider.find("http://") != std::string::npos || + video_stream_provider.find("https://") != std::string::npos){ + video_stream_provider_type = "http_stream"; + } + else if(video_stream_provider.find("rtsp://") != std::string::npos){ + video_stream_provider_type = "rtsp_stream"; + } + else { + // Check if file exists to know if it's a videofile + std::ifstream ifs; + ifs.open(video_stream_provider.c_str(), std::ifstream::in); + if (ifs.good()){ + video_stream_provider_type = "videofile"; + } + else + video_stream_provider_type = "unknown"; + } cap.open(video_stream_provider); } } @@ -137,59 +171,62 @@ int main(int argc, char** argv) return -1; } + ROS_INFO_STREAM("Video stream provider type detected: " << video_stream_provider_type << ncurses_cr); + + std::string camera_name; _nh.param("camera_name", camera_name, std::string("camera")); - ROS_INFO_STREAM("Camera name: " << camera_name); + ROS_INFO_STREAM("Camera name: " << camera_name << ncurses_cr); _nh.param("set_camera_fps", set_camera_fps, 30.0); - ROS_INFO_STREAM("Setting camera FPS to: " << set_camera_fps); - cap.set(CV_CAP_PROP_FPS, set_camera_fps); + ROS_INFO_STREAM("Setting camera FPS to: " << set_camera_fps << ncurses_cr); + cap.set(cv::CAP_PROP_FPS, set_camera_fps); double reported_camera_fps; // OpenCV 2.4 returns -1 (instead of a 0 as the spec says) and prompts an error // HIGHGUI ERROR: V4L2: Unable to get property (5) - Invalid argument - reported_camera_fps = cap.get(CV_CAP_PROP_FPS); + reported_camera_fps = cap.get(cv::CAP_PROP_FPS); if (reported_camera_fps > 0.0) - ROS_INFO_STREAM("Camera reports FPS: " << reported_camera_fps); + ROS_INFO_STREAM("Camera reports FPS: " << reported_camera_fps << ncurses_cr); else - ROS_INFO_STREAM("Backend can't provide camera FPS information"); + ROS_INFO_STREAM("Backend can't provide camera FPS information" << ncurses_cr); int buffer_queue_size; _nh.param("buffer_queue_size", buffer_queue_size, 100); - ROS_INFO_STREAM("Setting buffer size for capturing frames to: " << buffer_queue_size); + ROS_INFO_STREAM("Setting buffer size for capturing frames to: " << buffer_queue_size << ncurses_cr); max_queue_size = buffer_queue_size; double fps; _nh.param("fps", fps, 240.0); - ROS_INFO_STREAM("Throttling to fps: " << fps); + ROS_INFO_STREAM("Throttling to fps: " << fps << ncurses_cr); if (video_stream_provider.size() < 4 && fps > set_camera_fps) ROS_WARN_STREAM("Asked to publish at 'fps' (" << fps << ") which is higher than the 'set_camera_fps' (" << set_camera_fps << - "), we can't publish faster than the camera provides images."); + "), we can't publish faster than the camera provides images." << ncurses_cr); std::string frame_id; _nh.param("frame_id", frame_id, std::string("camera")); - ROS_INFO_STREAM("Publishing with frame_id: " << frame_id); + ROS_INFO_STREAM("Publishing with frame_id: " << frame_id << ncurses_cr); std::string camera_info_url; _nh.param("camera_info_url", camera_info_url, std::string("")); - ROS_INFO_STREAM("Provided camera_info_url: '" << camera_info_url << "'"); + ROS_INFO_STREAM("Provided camera_info_url: '" << camera_info_url << "'" << ncurses_cr); bool flip_horizontal; _nh.param("flip_horizontal", flip_horizontal, false); - ROS_INFO_STREAM("Flip horizontal image is: " << ((flip_horizontal)?"true":"false")); + ROS_INFO_STREAM("Flip horizontal image is: " << ((flip_horizontal)?"true":"false") << ncurses_cr); bool flip_vertical; _nh.param("flip_vertical", flip_vertical, false); - ROS_INFO_STREAM("Flip vertical image is: " << ((flip_vertical)?"true":"false")); + ROS_INFO_STREAM("Flip vertical image is: " << ((flip_vertical)?"true":"false") << ncurses_cr); int width_target; int height_target; _nh.param("width", width_target, 0); _nh.param("height", height_target, 0); if (width_target != 0 && height_target != 0){ - ROS_INFO_STREAM("Forced image width is: " << width_target); - ROS_INFO_STREAM("Forced image height is: " << height_target); + ROS_INFO_STREAM("Forced image width is: " << width_target << ncurses_cr); + ROS_INFO_STREAM("Forced image height is: " << height_target << ncurses_cr); } // From http://docs.opencv.org/modules/core/doc/operations_on_arrays.html#void flip(InputArray src, OutputArray dst, int flipCode) @@ -206,15 +243,16 @@ int main(int argc, char** argv) flip_image = false; if(!cap.isOpened()){ - ROS_ERROR_STREAM("Could not open the stream."); + ROS_ERROR_STREAM("Could not open the stream." << ncurses_cr); return -1; } if (width_target != 0 && height_target != 0){ - cap.set(CV_CAP_PROP_FRAME_WIDTH, width_target); - cap.set(CV_CAP_PROP_FRAME_HEIGHT, height_target); + cap.set(cv::CAP_PROP_FRAME_WIDTH, width_target); + cap.set(cv::CAP_PROP_FRAME_HEIGHT, height_target); } cv::Mat frame; + bool first_run = true; sensor_msgs::ImagePtr msg; sensor_msgs::CameraInfo cam_info_msg; std_msgs::Header header; @@ -222,13 +260,45 @@ int main(int argc, char** argv) camera_info_manager::CameraInfoManager cam_info_manager(nh, camera_name, camera_info_url); // Get the saved camera info if any cam_info_msg = cam_info_manager.getCameraInfo(); + cam_info_msg.header = header; - ROS_INFO_STREAM("Opened the stream, starting to publish."); + ROS_INFO_STREAM("Opened the stream, starting to publish." << ncurses_cr); boost::thread cap_thread(do_capture, nh); ros::Rate r(fps); - while (nh.ok()) { - framesQueue.pull(frame); + bool paused = false; + + while (nh.ok()) + { +#ifdef HAVE_NCURSES + char c = getch(); + if (c == 27) + break; + + switch (c) + { + case 'p': + paused = !paused; + if (paused) + ROS_INFO_STREAM("Pause" << ncurses_cr); + else + ROS_INFO_STREAM("Resuming..." << ncurses_cr); + break; + case -1: + /// Nothing pressed + break; + + default: + ROS_INFO_STREAM("Key pressed: " << c << ncurses_cr); + break; + } + + if (paused) + continue; +#endif + + if (!framesQueue.empty()) + framesQueue.pull(frame); if (pub.getNumSubscribers() > 0){ // Check if grabbed frame is actually filled with some content @@ -239,17 +309,34 @@ int main(int argc, char** argv) msg = cv_bridge::CvImage(header, "bgr8", frame).toImageMsg(); // Create a default camera info if we didn't get a stored one on initialization if (cam_info_msg.distortion_model == ""){ - ROS_WARN_STREAM("No calibration file given, publishing a reasonable default camera info."); + ROS_WARN_STREAM("No calibration file given, publishing a reasonable default camera info." << ncurses_cr); cam_info_msg = get_default_camera_info_from_image(msg); cam_info_manager.setCameraInfo(cam_info_msg); } + + if (first_run) { + first_run = false; + uint64_t memusage = 1.0 * max_queue_size * (uint64_t(frame.dataend) - uint64_t(frame.datastart)) / 1048576; + // MB of RAM + if (memusage > 512) { + ROS_WARN_STREAM("WARNING. Queue size: " << max_queue_size << " * allocated image size: " + << uint64_t(frame.dataend) - uint64_t(frame.datastart) + << " may occupy up to: " << memusage << " MB of RAM"); + ROS_WARN_STREAM("especially on slow CPUs like on Raspberry Pi where exact timings cannot be met."); + ROS_WARN_STREAM("Reduce the queue size if memory consumption becomes too high."); + } + } + // The timestamps are in sync thanks to this publisher pub.publish(*msg, cam_info_msg, ros::Time::now()); } - ros::spinOnce(); } r.sleep(); } cap_thread.join(); + +#ifdef HAVE_NCURSES + endwin(); +#endif }