From 77a0db1551d6f0e96cc9e69bc1982107d99336c3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Timo=20R=C3=B6hling?= Date: Wed, 18 Sep 2019 17:55:13 +0200 Subject: [PATCH] Implement logic to resize or drop frames on demand --- .../include/webrtc_ros/ros_video_capturer.h | 2 +- webrtc_ros/include/webrtc_ros/webrtc_client.h | 4 +- webrtc_ros/src/ros_video_capturer.cpp | 111 +++++++++--------- webrtc_ros/src/webrtc_client.cpp | 9 +- 4 files changed, 65 insertions(+), 61 deletions(-) diff --git a/webrtc_ros/include/webrtc_ros/ros_video_capturer.h b/webrtc_ros/include/webrtc_ros/ros_video_capturer.h index bc43909..5c73e24 100644 --- a/webrtc_ros/include/webrtc_ros/ros_video_capturer.h +++ b/webrtc_ros/include/webrtc_ros/ros_video_capturer.h @@ -22,7 +22,7 @@ class RosVideoCapturer : RosVideoCapturer(const ImageTransportFactory& it, const std::string& topic, const std::string& transport); virtual ~RosVideoCapturer(); - void imageCallback(const std::shared_ptr& frame); + void imageCallback(const sensor_msgs::ImageConstPtr& msg); virtual cricket::CaptureState Start(const cricket::VideoFormat& capture_format) override; virtual void Stop() override; diff --git a/webrtc_ros/include/webrtc_ros/webrtc_client.h b/webrtc_ros/include/webrtc_ros/webrtc_client.h index e76846d..d7dc128 100644 --- a/webrtc_ros/include/webrtc_ros/webrtc_client.h +++ b/webrtc_ros/include/webrtc_ros/webrtc_client.h @@ -65,7 +65,7 @@ class WebrtcClient : private boost::noncopyable bool initPeerConnection(); - void ping_timer_callback(const ros::TimerEvent&); + void ping_timer_callback(const ros::WallTimerEvent&); void handle_message(MessageHandler::Type type, const std::string& message); @@ -92,7 +92,7 @@ class WebrtcClient : private boost::noncopyable std::map> expected_streams_; - ros::Timer ping_timer_; + ros::WallTimer ping_timer_; friend WebrtcClientObserverProxy; friend MessageHandlerImpl; diff --git a/webrtc_ros/src/ros_video_capturer.cpp b/webrtc_ros/src/ros_video_capturer.cpp index c4db1d4..0d2d2e6 100644 --- a/webrtc_ros/src/ros_video_capturer.cpp +++ b/webrtc_ros/src/ros_video_capturer.cpp @@ -15,17 +15,6 @@ namespace webrtc_ros RosVideoCapturer::RosVideoCapturer(const ImageTransportFactory& it, const std::string& topic, const std::string& transport) : impl_(new RosVideoCapturerImpl(it, topic, transport)) { - - // Default supported formats. Use ResetSupportedFormats to over write. - std::vector formats; - formats.push_back(cricket::VideoFormat(1280, 720, - cricket::VideoFormat::FpsToInterval(30), cricket::FOURCC_I420)); - formats.push_back(cricket::VideoFormat(640, 480, - cricket::VideoFormat::FpsToInterval(30), cricket::FOURCC_I420)); - formats.push_back(cricket::VideoFormat(320, 240, - cricket::VideoFormat::FpsToInterval(30), cricket::FOURCC_I420)); - formats.push_back(cricket::VideoFormat(160, 120, - cricket::VideoFormat::FpsToInterval(30), cricket::FOURCC_I420)); } @@ -56,14 +45,63 @@ void RosVideoCapturer::Stop() SetCaptureState(cricket::CS_STOPPED); } -void RosVideoCapturer::imageCallback(const std::shared_ptr& frame) +void RosVideoCapturer::imageCallback(const sensor_msgs::ImageConstPtr& msg) { - // Apparently, the OnFrame() method could not be called from arbitrary threads in ancient times, and there - // used to be all kinds of shenanigans here to make sure it is called from the original thread, causing - // a subtle deadlock bug on object destruction. - // - // So I decided to be blunt and just call it like it is: - OnFrame(*frame, frame->width(), frame->height()); + cv::Mat bgr; + if (msg->encoding.find("F") != std::string::npos) + { + // scale floating point images + cv::Mat float_image_bridge = cv_bridge::toCvShare(msg, msg->encoding)->image; + cv::Mat_ float_image = float_image_bridge; + double max_val; + cv::minMaxIdx(float_image, 0, &max_val); + + if (max_val > 0) + { + float_image *= (255 / max_val); + } + cv::Mat orig; + float_image.convertTo(orig, CV_8U); + cv::cvtColor(orig, bgr, CV_GRAY2BGR); + } + else + { + bgr = cv_bridge::toCvShare(msg, "bgr8")->image; + } + int64_t camera_time_us = msg->header.stamp.toNSec() / 1000; + int64_t system_time_us = ros::WallTime::now().toNSec() / 1000; + cv::Rect roi; + int out_width, out_height; + int64_t translated_camera_time_us; + if (AdaptFrame(bgr.cols, bgr.rows, camera_time_us, system_time_us, &out_width, &out_height, &roi.width, &roi.height, &roi.x, &roi.y, &translated_camera_time_us)) + { + cv::Mat yuv; + if (out_width == roi.width && out_height == roi.height) + { + cv::cvtColor(bgr(roi), yuv, CV_BGR2YUV_I420); + } + else + { + cv::Mat m; + cv::resize(bgr(roi), m, cv::Size2i(out_width, out_height), 0, 0, out_width < roi.width ? cv::INTER_AREA : cv::INTER_LINEAR); + cv::cvtColor(m, yuv, CV_BGR2YUV_I420); + } + uint8_t* y = yuv.data; + uint8_t* u = y + (out_width * out_height); + uint8_t* v = u + (out_width * out_height) / 4; + + std::shared_ptr frame = std::make_shared( + webrtc::I420Buffer::Copy(out_width, out_height, y, out_width, u, out_width / 2, v, out_width / 2), + webrtc::kVideoRotation_0, + translated_camera_time_us + ); + // Apparently, the OnFrame() method could not be called from arbitrary threads in ancient times, and there + // used to be all kinds of shenanigans here to make sure it is called from the original thread, causing + // a subtle deadlock bug on object destruction. + // + // So I decided to be blunt and just call it like it is: + OnFrame(*frame, frame->width(), frame->height()); + } } bool RosVideoCapturer::IsRunning() @@ -133,42 +171,7 @@ void RosVideoCapturerImpl::imageCallback(const sensor_msgs::ImageConstPtr& msg) std::unique_lock lock(state_mutex_); if(capturer_ == nullptr) return; - cv::Mat bgr; - if (msg->encoding.find("F") != std::string::npos) - { - // scale floating point images - cv::Mat float_image_bridge = cv_bridge::toCvCopy(msg, msg->encoding)->image; - cv::Mat_ float_image = float_image_bridge; - double max_val; - cv::minMaxIdx(float_image, 0, &max_val); - - if (max_val > 0) - { - float_image *= (255 / max_val); - } - cv::Mat orig; - float_image.convertTo(orig, CV_8U); - cv::cvtColor(orig, bgr, CV_GRAY2BGR); - } - else - { - bgr = cv_bridge::toCvCopy(msg, "bgr8")->image; - } - - cv::Mat yuv; - cv::cvtColor(bgr, yuv, CV_BGR2YUV_I420); - - uint8_t* y = yuv.data; - uint8_t* u = y + (bgr.cols * bgr.rows); - uint8_t* v = u + (bgr.cols * bgr.rows) / 4; - - std::shared_ptr frame = std::make_shared( - webrtc::I420Buffer::Copy(bgr.cols, bgr.rows, y, bgr.cols, u, bgr.cols / 2, v, bgr.cols / 2), - msg->header.stamp.toNSec() / 1000000, - msg->header.stamp.toNSec() / 1000000, - webrtc::kVideoRotation_0 - ); - capturer_->imageCallback(frame); + capturer_->imageCallback(msg); } } diff --git a/webrtc_ros/src/webrtc_client.cpp b/webrtc_ros/src/webrtc_client.cpp index 3bd68d2..bd68101 100644 --- a/webrtc_ros/src/webrtc_client.cpp +++ b/webrtc_ros/src/webrtc_client.cpp @@ -80,7 +80,7 @@ WebrtcClient::WebrtcClient(ros::NodeHandle& nh, const ImageTransportFactory& itf peer_connection_constraints_.SetAllowDtlsSctpDataChannels(); media_constraints_.AddOptional(webrtc::MediaConstraintsInterface::kOfferToReceiveAudio, true); media_constraints_.AddOptional(webrtc::MediaConstraintsInterface::kOfferToReceiveVideo, true); - ping_timer_ = nh_.createTimer(ros::Duration(10.0), boost::bind(&WebrtcClient::ping_timer_callback, this, _1)); + ping_timer_ = nh_.createWallTimer(ros::WallDuration(10.0), boost::bind(&WebrtcClient::ping_timer_callback, this, _1)); } WebrtcClient::~WebrtcClient() { @@ -154,7 +154,7 @@ MessageHandler* WebrtcClient::createMessageHandler() return new MessageHandlerImpl(keep_alive_this_); } -void WebrtcClient::ping_timer_callback(const ros::TimerEvent& event) +void WebrtcClient::ping_timer_callback(const ros::WallTimerEvent& event) { try { @@ -213,6 +213,7 @@ void WebrtcClient::handle_message(MessageHandler::Type type, const std::string& { Json::Reader reader; Json::Value message_json; + ROS_INFO("JSON: %s", raw.c_str()); if (!reader.parse(raw, message_json)) { ROS_WARN_STREAM("Could not parse message: " << raw); @@ -300,7 +301,7 @@ void WebrtcClient::handle_message(MessageHandler::Type type, const std::string& stream->AddTrack(video_track); } else { - ROS_WARN_STREAM("Unknwon video source type: " << video_type); + ROS_WARN_STREAM("Unknown video source type: " << video_type); } } @@ -330,7 +331,7 @@ void WebrtcClient::handle_message(MessageHandler::Type type, const std::string& stream->AddTrack(audio_track); } else { - ROS_WARN_STREAM("Unknwon video source type: " << audio_type); + ROS_WARN_STREAM("Unknown video source type: " << audio_type); } }